From cce521af39ace3b5150f381ba80f4ae2377653d5 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 27 Sep 2019 16:26:16 -0700 Subject: [PATCH 01/31] Added two structs: rmw_participant_qos_profile_t: stores the publisher or subscriber name and its respective qos profile rmw_participants_t : represents a list of rmw_participants_qos_profile_t Signed-off-by: Jaison Titus --- rmw/include/rmw/types.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index c68e00f7..446cd195 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -327,6 +327,25 @@ typedef struct RMW_PUBLIC_TYPE rmw_qos_profile_t bool avoid_ros_namespace_conventions; } rmw_qos_profile_t; +/// Structure to store the name of the participant and its respective qos profile +typedef struct RMW_PUBLIC_TYPE rmw_participant_qos_profile_t +{ + /// Name of the participant + const char * participant_name; + /// Qos profile of the participant + const rmw_qos_profile_t * qos_profile; +} rmw_participant_qos_profile_t; + +/// Array of rmw_participant_qos_profile_t +typedef struct RMW_PUBLIC_TYPE rmw_participants_t +{ + /// The number of participants represented by the array. + size_t count; + /// Pointer to an array of void * pointers of participants of type rmw_participant_qos_profile_t. + void ** participants; +} rmw_participants_t; + + typedef struct RMW_PUBLIC_TYPE rmw_gid_t { const char * implementation_identifier; From 8fe5fd2220f38b823bdd258853d237234016b15f Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 27 Sep 2019 16:29:33 -0700 Subject: [PATCH 02/31] Added two functions to retrieve the list of all subscribers and publishers to a topic, along with their respective qos profiles. Signed-off-by: Jaison Titus --- rmw/include/rmw/rmw.h | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index ce8ab8f8..174d661e 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1051,6 +1051,44 @@ rmw_count_publishers( const char * topic_name, size_t * count); +/** + * This function can be used to retrieve a list of all publishers (described by the rmw_participants_t struct) + * publishing to a specific topic along with its respective qos profile. + * None of the parameters provided to this function can be NULL. + * Incorrect or non existent topic names are allowed. + * \param[in] node the handle to the node being used to query the ROS graph. + * \param[in] topic_name the name of the topic for which the list of publishers will be retrieved. + * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. + * \return `RMW_RET_ERROR` if any of the parameters are NULL. + * \return `RMW_RET_ERROR` if an unspecified error occurs + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_get_qos_for_publishers( + const rmw_node_t * node, + const char * topic_name, + rmw_participants_t * publishers); + +/** + * This function can be used to retrieve a list of all subscribers (described by the rmw_participants_t struct) + * subscribing to a specific topic along with its respective qos profile. + * None of the parameters provided to this function can be NULL. + * Incorrect or non existent topic names are allowed. + * \param[in] node the handle to the node being used to query the ROS graph. + * \param[in] topic_name the name of the topic for which the list of subscribers will be retrieved. + * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. + * \return `RMW_RET_ERROR` if any of the parameters are NULL. + * \return `RMW_RET_ERROR` if an unspecified error occurs + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_get_qos_for_subscribers( + const rmw_node_t * node, + const char * topic_name, + rmw_participants_t * subscribers); + RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t From 69744ddfe09d22063aea9c27272efe6eca096b6f Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 27 Sep 2019 16:34:32 -0700 Subject: [PATCH 03/31] Allocator and free functions for rmw_participant_qos_profile Signed-off-by: Jaison Titus --- rmw/include/rmw/allocators.h | 15 +++++++++++++++ rmw/src/allocators.c | 20 ++++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/rmw/include/rmw/allocators.h b/rmw/include/rmw/allocators.h index 3eec025d..c10e6803 100644 --- a/rmw/include/rmw/allocators.h +++ b/rmw/include/rmw/allocators.h @@ -87,6 +87,21 @@ RMW_PUBLIC void rmw_wait_set_free(rmw_wait_set_t * wait_set); +/** +* @return an allocated instance of rmw_participant_qos_profile_t. + */ +RMW_PUBLIC +rmw_participant_qos_profile_t * +rmw_participant_qos_profile_allocate(void); + +/** + * Frees the allocated instance. + * @param participant_qos_profile the instance to free. + */ +RMW_PUBLIC +void +rmw_participant_qos_profile_free(rmw_participant_qos_profile_t * participant_qos_profile); + #ifdef __cplusplus } #endif diff --git a/rmw/src/allocators.c b/rmw/src/allocators.c index 55ba070f..dfd521ee 100644 --- a/rmw/src/allocators.c +++ b/rmw/src/allocators.c @@ -139,3 +139,23 @@ rmw_wait_set_free(rmw_wait_set_t * wait_set) // Should have matching overide with rmw_wait_set_allocate rmw_free(wait_set); } + +/** +* @return an allocated instance of rmw_participant_qos_profile_t. + */ +rmw_participant_qos_profile_t * +rmw_participant_qos_profile_allocate() +{ + return (rmw_participant_qos_profile_t *)rmw_allocate(sizeof(rmw_participant_qos_profile_t)); +} + +/** + * Frees the allocated instance. + * @param participant_qos_profile the instance to free. + */ +void +rmw_participant_qos_profile_free(rmw_participant_qos_profile_t * participant_qos_profile) +{ + rmw_free(participant_qos_profile); +} + From 20bedc228b07384069232f92de752a686c2afc6e Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 27 Sep 2019 17:09:31 -0700 Subject: [PATCH 04/31] Fixed code indentation issues Signed-off-by: Jaison Titus --- rmw/include/rmw/rmw.h | 24 ++++++++++++++++-------- rmw/include/rmw/types.h | 2 +- rmw/src/allocators.c | 1 - 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index 174d661e..8bd22f14 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1052,12 +1052,16 @@ rmw_count_publishers( size_t * count); /** - * This function can be used to retrieve a list of all publishers (described by the rmw_participants_t struct) + * Retrieves a list of all publishers (described by the rmw_participants_t struct) * publishing to a specific topic along with its respective qos profile. + * * None of the parameters provided to this function can be NULL. + * * Incorrect or non existent topic names are allowed. + * * \param[in] node the handle to the node being used to query the ROS graph. * \param[in] topic_name the name of the topic for which the list of publishers will be retrieved. + * \param[out] publishers an array of publishers. * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. * \return `RMW_RET_ERROR` if any of the parameters are NULL. * \return `RMW_RET_ERROR` if an unspecified error occurs @@ -1066,17 +1070,21 @@ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t rmw_get_qos_for_publishers( - const rmw_node_t * node, - const char * topic_name, - rmw_participants_t * publishers); + const rmw_node_t * node, + const char * topic_name, + rmw_participants_t * publishers); /** - * This function can be used to retrieve a list of all subscribers (described by the rmw_participants_t struct) + * Retrieves a list of all subscribers (described by the rmw_participants_t struct) * subscribing to a specific topic along with its respective qos profile. + * * None of the parameters provided to this function can be NULL. + * * Incorrect or non existent topic names are allowed. + * * \param[in] node the handle to the node being used to query the ROS graph. * \param[in] topic_name the name of the topic for which the list of subscribers will be retrieved. + * \param[out] subscribers an array of subscribers. * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. * \return `RMW_RET_ERROR` if any of the parameters are NULL. * \return `RMW_RET_ERROR` if an unspecified error occurs @@ -1085,9 +1093,9 @@ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t rmw_get_qos_for_subscribers( - const rmw_node_t * node, - const char * topic_name, - rmw_participants_t * subscribers); + const rmw_node_t * node, + const char * topic_name, + rmw_participants_t * subscribers); RMW_PUBLIC RMW_WARN_UNUSED diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index 446cd195..36a5f309 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -327,7 +327,7 @@ typedef struct RMW_PUBLIC_TYPE rmw_qos_profile_t bool avoid_ros_namespace_conventions; } rmw_qos_profile_t; -/// Structure to store the name of the participant and its respective qos profile +/// Stores participant name and its qos profile typedef struct RMW_PUBLIC_TYPE rmw_participant_qos_profile_t { /// Name of the participant diff --git a/rmw/src/allocators.c b/rmw/src/allocators.c index dfd521ee..8f554c98 100644 --- a/rmw/src/allocators.c +++ b/rmw/src/allocators.c @@ -158,4 +158,3 @@ rmw_participant_qos_profile_free(rmw_participant_qos_profile_t * participant_qos { rmw_free(participant_qos_profile); } - From 94b27b32dce6b6c91e1c2f3b0ebdf45861f216da Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 3 Oct 2019 18:17:23 -0700 Subject: [PATCH 05/31] Tests to rmw_participant_qos_profile_t_allocator Signed-off-by: Jaison Titus --- rmw/test/CMakeLists.txt | 9 +++++ ..._rmw_participant_qos_profile_allocator.cpp | 38 +++++++++++++++++++ 2 files changed, 47 insertions(+) create mode 100644 rmw/test/test_rmw_participant_qos_profile_allocator.cpp diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index c6a12710..95645a71 100644 --- a/rmw/test/CMakeLists.txt +++ b/rmw/test/CMakeLists.txt @@ -41,3 +41,12 @@ if(TARGET test_validate_namespace) set_target_properties(test_validate_namespace PROPERTIES COMPILE_FLAGS "-std=c++14") endif() endif() + +ament_add_gmock(test_rmw_participant_qos_profile_allocator + test_rmw_participant_qos_profile_allocator.cpp + # Append the directory of librmw so it is found at test time. + APPEND_LIBRARY_DIRS "$" +) +if(TARGET test_rmw_participant_qos_profile_allocator) + target_link_libraries(test_rmw_participant_qos_profile_allocator ${PROJECT_NAME}) +endif() diff --git a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp new file mode 100644 index 00000000..67bf5ae0 --- /dev/null +++ b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp @@ -0,0 +1,38 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 "gtest/gtest.h" + +#include "rmw/allocators.h" +#include "rmw/types.h" + +TEST(test_rmw_participant_qos_profile_allocator, test_allocate_does_not_return_null) { + rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); + ASSERT_TRUE(qos_profile != NULL); +} + +TEST(test_rmw_participant_qos_profile_allocator, test_allocate_allocates_different_pointers) { + rmw_participant_qos_profile_t * qos_profile1 = rmw_participant_qos_profile_allocate(); + rmw_participant_qos_profile_t * qos_profile2 = rmw_participant_qos_profile_allocate(); + ASSERT_TRUE(qos_profile1 != qos_profile2); +} + +TEST(test_rmw_participant_qos_profile_allocator, test_free_null) { + EXPECT_NO_THROW(rmw_participant_qos_profile_free(NULL)); +} + +TEST(test_rmw_participant_qos_profile_allocator, test_free_allocated) { + rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); + EXPECT_NO_THROW(rmw_participant_qos_profile_free(qos_profile)); +} From ed9651ed2ba1db18c15eadc9e545d490817a8e14 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 4 Oct 2019 10:48:48 -0700 Subject: [PATCH 06/31] Modified tests to avoid memory leak Signed-off-by: Jaison Titus --- rmw/include/rmw/rmw.h | 13 ++++++------ rmw/package.xml | 2 +- rmw/test/CMakeLists.txt | 4 +++- ..._rmw_participant_qos_profile_allocator.cpp | 20 ++++++++++++++----- 4 files changed, 26 insertions(+), 13 deletions(-) diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index 8bd22f14..d88366c5 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1052,12 +1052,12 @@ rmw_count_publishers( size_t * count); /** - * Retrieves a list of all publishers (described by the rmw_participants_t struct) - * publishing to a specific topic along with its respective qos profile. + * Retrieves a list of all publishers publishing to a specific topic along with its respective qos profile. * - * None of the parameters provided to this function can be NULL. + * The node parameter must not be `NULL` and must point to a valid node. * - * Incorrect or non existent topic names are allowed. + * The topic_name parameter must not be `NULL`. + * Incorrect or non existent topic names are allowed. They will return an empty array. * * \param[in] node the handle to the node being used to query the ROS graph. * \param[in] topic_name the name of the topic for which the list of publishers will be retrieved. @@ -1078,9 +1078,10 @@ rmw_get_qos_for_publishers( * Retrieves a list of all subscribers (described by the rmw_participants_t struct) * subscribing to a specific topic along with its respective qos profile. * - * None of the parameters provided to this function can be NULL. + * The node parameter must not be `NULL` and must point to a valid node. * - * Incorrect or non existent topic names are allowed. + * The topic_name parameter must not be `NULL`. + * Incorrect or non existent topic names are allowed. They will return an empty array. * * \param[in] node the handle to the node being used to query the ROS graph. * \param[in] topic_name the name of the topic for which the list of subscribers will be retrieved. diff --git a/rmw/package.xml b/rmw/package.xml index 50d89b10..525500c3 100644 --- a/rmw/package.xml +++ b/rmw/package.xml @@ -20,7 +20,7 @@ ament_cmake_gmock ament_lint_auto ament_lint_common - + osrf_testing_tools_cpp ament_cmake diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index 95645a71..fb7a54ba 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 @@ -48,5 +49,6 @@ ament_add_gmock(test_rmw_participant_qos_profile_allocator APPEND_LIBRARY_DIRS "$" ) if(TARGET test_rmw_participant_qos_profile_allocator) - target_link_libraries(test_rmw_participant_qos_profile_allocator ${PROJECT_NAME}) + target_link_libraries(test_rmw_participant_qos_profile_allocator ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools) endif() diff --git a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp index 67bf5ae0..8c35b232 100644 --- a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp +++ b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp @@ -1,4 +1,4 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright 2019 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,25 +14,35 @@ #include "gtest/gtest.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rmw/allocators.h" #include "rmw/types.h" TEST(test_rmw_participant_qos_profile_allocator, test_allocate_does_not_return_null) { rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); - ASSERT_TRUE(qos_profile != NULL); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + delete (qos_profile); + }); + EXPECT_NE(qos_profile, nullptr); } TEST(test_rmw_participant_qos_profile_allocator, test_allocate_allocates_different_pointers) { rmw_participant_qos_profile_t * qos_profile1 = rmw_participant_qos_profile_allocate(); rmw_participant_qos_profile_t * qos_profile2 = rmw_participant_qos_profile_allocate(); - ASSERT_TRUE(qos_profile1 != qos_profile2); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + delete (qos_profile1); + delete (qos_profile2); + }); + EXPECT_NE(qos_profile1, qos_profile2); } TEST(test_rmw_participant_qos_profile_allocator, test_free_null) { - EXPECT_NO_THROW(rmw_participant_qos_profile_free(NULL)); + rmw_participant_qos_profile_free(NULL); + SUCCEED(); } TEST(test_rmw_participant_qos_profile_allocator, test_free_allocated) { rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); - EXPECT_NO_THROW(rmw_participant_qos_profile_free(qos_profile)); + rmw_participant_qos_profile_free(qos_profile); + SUCCEED(); } From 5c5ccbf7dea6c4c03dc8d1afca485eaa2d0ee207 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 8 Oct 2019 11:16:22 -0700 Subject: [PATCH 07/31] - Updated rmw_participants_t to use rmw_participant_qos_profile_t * instead of a generic pointer - Updated tests to use the correct deallocator Signed-off-by: Jaison Titus --- rmw/include/rmw/types.h | 5 ++--- rmw/test/test_rmw_participant_qos_profile_allocator.cpp | 6 +++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index 36a5f309..8f856f15 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -341,11 +341,10 @@ typedef struct RMW_PUBLIC_TYPE rmw_participants_t { /// The number of participants represented by the array. size_t count; - /// Pointer to an array of void * pointers of participants of type rmw_participant_qos_profile_t. - void ** participants; + /// Pointer to an array of rmw_participant_qos_profile_t *. + rmw_participant_qos_profile_t ** participants; } rmw_participants_t; - typedef struct RMW_PUBLIC_TYPE rmw_gid_t { const char * implementation_identifier; diff --git a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp index 8c35b232..621e4eec 100644 --- a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp +++ b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp @@ -21,7 +21,7 @@ TEST(test_rmw_participant_qos_profile_allocator, test_allocate_does_not_return_null) { rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - delete (qos_profile); + rmw_participant_qos_profile_free(qos_profile); }); EXPECT_NE(qos_profile, nullptr); } @@ -30,8 +30,8 @@ TEST(test_rmw_participant_qos_profile_allocator, test_allocate_allocates_differe rmw_participant_qos_profile_t * qos_profile1 = rmw_participant_qos_profile_allocate(); rmw_participant_qos_profile_t * qos_profile2 = rmw_participant_qos_profile_allocate(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - delete (qos_profile1); - delete (qos_profile2); + rmw_participant_qos_profile_free(qos_profile1); + rmw_participant_qos_profile_free(qos_profile2); }); EXPECT_NE(qos_profile1, qos_profile2); } From 06c5d4cb569a1086134fbbead5dccca37fa97628 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 8 Oct 2019 16:03:38 -0700 Subject: [PATCH 08/31] Changed rmw_participants_t to use a pointer to an array. Signed-off-by: Jaison Titus --- rmw/include/rmw/types.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index 8f856f15..bdaa43da 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -341,8 +341,8 @@ typedef struct RMW_PUBLIC_TYPE rmw_participants_t { /// The number of participants represented by the array. size_t count; - /// Pointer to an array of rmw_participant_qos_profile_t *. - rmw_participant_qos_profile_t ** participants; + /// Pointer representing an array of rmw_participant_qos_profile_t. + rmw_participant_qos_profile_t * participants; } rmw_participants_t; typedef struct RMW_PUBLIC_TYPE rmw_gid_t From a5e113bb4eb4b9d2c255d517bc9eea3e43ee9007 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 15 Oct 2019 14:29:47 -0700 Subject: [PATCH 09/31] PR rework after design changes - Removed allocator and free for rmw_participant_qos_profile_t - Added new structs to better represent required data - Added functions to get publisher and subscriber info by topic - Removed previously created tests for the allocator and free functions Signed-off-by: Jaison Titus --- rmw/include/rmw/allocators.h | 15 --- rmw/include/rmw/rmw.h | 111 ++++++++++-------- rmw/include/rmw/types.h | 43 ++++--- rmw/package.xml | 2 +- rmw/src/allocators.c | 19 --- rmw/test/CMakeLists.txt | 11 -- ..._rmw_participant_qos_profile_allocator.cpp | 48 -------- 7 files changed, 90 insertions(+), 159 deletions(-) delete mode 100644 rmw/test/test_rmw_participant_qos_profile_allocator.cpp diff --git a/rmw/include/rmw/allocators.h b/rmw/include/rmw/allocators.h index c10e6803..3eec025d 100644 --- a/rmw/include/rmw/allocators.h +++ b/rmw/include/rmw/allocators.h @@ -87,21 +87,6 @@ RMW_PUBLIC void rmw_wait_set_free(rmw_wait_set_t * wait_set); -/** -* @return an allocated instance of rmw_participant_qos_profile_t. - */ -RMW_PUBLIC -rmw_participant_qos_profile_t * -rmw_participant_qos_profile_allocate(void); - -/** - * Frees the allocated instance. - * @param participant_qos_profile the instance to free. - */ -RMW_PUBLIC -void -rmw_participant_qos_profile_free(rmw_participant_qos_profile_t * participant_qos_profile); - #ifdef __cplusplus } #endif diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index d88366c5..c81f9e62 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1051,53 +1051,6 @@ rmw_count_publishers( const char * topic_name, size_t * count); -/** - * Retrieves a list of all publishers publishing to a specific topic along with its respective qos profile. - * - * The node parameter must not be `NULL` and must point to a valid node. - * - * The topic_name parameter must not be `NULL`. - * Incorrect or non existent topic names are allowed. They will return an empty array. - * - * \param[in] node the handle to the node being used to query the ROS graph. - * \param[in] topic_name the name of the topic for which the list of publishers will be retrieved. - * \param[out] publishers an array of publishers. - * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. - * \return `RMW_RET_ERROR` if any of the parameters are NULL. - * \return `RMW_RET_ERROR` if an unspecified error occurs - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_get_qos_for_publishers( - const rmw_node_t * node, - const char * topic_name, - rmw_participants_t * publishers); - -/** - * Retrieves a list of all subscribers (described by the rmw_participants_t struct) - * subscribing to a specific topic along with its respective qos profile. - * - * The node parameter must not be `NULL` and must point to a valid node. - * - * The topic_name parameter must not be `NULL`. - * Incorrect or non existent topic names are allowed. They will return an empty array. - * - * \param[in] node the handle to the node being used to query the ROS graph. - * \param[in] topic_name the name of the topic for which the list of subscribers will be retrieved. - * \param[out] subscribers an array of subscribers. - * \return `RMW_RET_OK` if the list of publishers was retrieved successfully. - * \return `RMW_RET_ERROR` if any of the parameters are NULL. - * \return `RMW_RET_ERROR` if an unspecified error occurs - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_get_qos_for_subscribers( - const rmw_node_t * node, - const char * topic_name, - rmw_participants_t * subscribers); - RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t @@ -1156,6 +1109,70 @@ RMW_WARN_UNUSED rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity); +/// Retrieves the information for all publishers to a given topic. +/** + * The retrieved information will contain the publisher's node name, node namespace, + * associated topic type, 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 + * Non existent topic names are allowed. They will return an empty array. + * + * \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_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_info_array_t * publishers_info); + +/// Retrieves the information for all subscriptions to a given topic. +/** + * The retrieved information will contain the subscriptions's node name, node namespace, + * associated topic type, 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 + * Non existent topic names are allowed. They will return an empty array. + * + * \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_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_info_array_t * subscriptions_info); + #ifdef __cplusplus } #endif diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index bdaa43da..2d2d7392 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -327,24 +327,6 @@ typedef struct RMW_PUBLIC_TYPE rmw_qos_profile_t bool avoid_ros_namespace_conventions; } rmw_qos_profile_t; -/// Stores participant name and its qos profile -typedef struct RMW_PUBLIC_TYPE rmw_participant_qos_profile_t -{ - /// Name of the participant - const char * participant_name; - /// Qos profile of the participant - const rmw_qos_profile_t * qos_profile; -} rmw_participant_qos_profile_t; - -/// Array of rmw_participant_qos_profile_t -typedef struct RMW_PUBLIC_TYPE rmw_participants_t -{ - /// The number of participants represented by the array. - size_t count; - /// Pointer representing an array of rmw_participant_qos_profile_t. - rmw_participant_qos_profile_t * participants; -} rmw_participants_t; - typedef struct RMW_PUBLIC_TYPE rmw_gid_t { const char * implementation_identifier; @@ -446,6 +428,31 @@ typedef struct RMW_PUBLIC_TYPE rmw_offered_deadline_missed_status_t int32_t total_count_change; } rmw_offered_deadline_missed_status_t; +/// 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_info_t +{ + /// Name of the node + const char * const node_name; + /// Namespace of the node + const char * const node_namespace; + /// The associated TopicType + const char * const topic_type; + /// The GID of the node + const rmw_gid_t * gid; + /// Qos profile of the node + const rmw_qos_profile_t * qos_profile; +} rmw_topic_info_t; + +/// Array of rmw_topic_info_t +typedef struct RMW_PUBLIC_TYPE rmw_topic_info_array_t +{ + /// Size of the array. + size_t count; + /// Pointer representing an array of rmw_topic_info_t + rmw_topic_info_t * info_array; +} rmw_topic_info_array_t; + #ifdef __cplusplus } #endif diff --git a/rmw/package.xml b/rmw/package.xml index 525500c3..50d89b10 100644 --- a/rmw/package.xml +++ b/rmw/package.xml @@ -20,7 +20,7 @@ ament_cmake_gmock ament_lint_auto ament_lint_common - osrf_testing_tools_cpp + ament_cmake diff --git a/rmw/src/allocators.c b/rmw/src/allocators.c index 8f554c98..55ba070f 100644 --- a/rmw/src/allocators.c +++ b/rmw/src/allocators.c @@ -139,22 +139,3 @@ rmw_wait_set_free(rmw_wait_set_t * wait_set) // Should have matching overide with rmw_wait_set_allocate rmw_free(wait_set); } - -/** -* @return an allocated instance of rmw_participant_qos_profile_t. - */ -rmw_participant_qos_profile_t * -rmw_participant_qos_profile_allocate() -{ - return (rmw_participant_qos_profile_t *)rmw_allocate(sizeof(rmw_participant_qos_profile_t)); -} - -/** - * Frees the allocated instance. - * @param participant_qos_profile the instance to free. - */ -void -rmw_participant_qos_profile_free(rmw_participant_qos_profile_t * participant_qos_profile) -{ - rmw_free(participant_qos_profile); -} diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index fb7a54ba..c6a12710 100644 --- a/rmw/test/CMakeLists.txt +++ b/rmw/test/CMakeLists.txt @@ -1,5 +1,4 @@ find_package(ament_cmake_gmock REQUIRED) -find_package(osrf_testing_tools_cpp REQUIRED) ament_add_gmock(test_serialized_message test_serialized_message.cpp @@ -42,13 +41,3 @@ if(TARGET test_validate_namespace) set_target_properties(test_validate_namespace PROPERTIES COMPILE_FLAGS "-std=c++14") endif() endif() - -ament_add_gmock(test_rmw_participant_qos_profile_allocator - test_rmw_participant_qos_profile_allocator.cpp - # Append the directory of librmw so it is found at test time. - APPEND_LIBRARY_DIRS "$" -) -if(TARGET test_rmw_participant_qos_profile_allocator) - target_link_libraries(test_rmw_participant_qos_profile_allocator ${PROJECT_NAME} - osrf_testing_tools_cpp::memory_tools) -endif() diff --git a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp b/rmw/test/test_rmw_participant_qos_profile_allocator.cpp deleted file mode 100644 index 621e4eec..00000000 --- a/rmw/test/test_rmw_participant_qos_profile_allocator.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// 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 "gtest/gtest.h" - -#include "osrf_testing_tools_cpp/scope_exit.hpp" -#include "rmw/allocators.h" -#include "rmw/types.h" - -TEST(test_rmw_participant_qos_profile_allocator, test_allocate_does_not_return_null) { - rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - rmw_participant_qos_profile_free(qos_profile); - }); - EXPECT_NE(qos_profile, nullptr); -} - -TEST(test_rmw_participant_qos_profile_allocator, test_allocate_allocates_different_pointers) { - rmw_participant_qos_profile_t * qos_profile1 = rmw_participant_qos_profile_allocate(); - rmw_participant_qos_profile_t * qos_profile2 = rmw_participant_qos_profile_allocate(); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - rmw_participant_qos_profile_free(qos_profile1); - rmw_participant_qos_profile_free(qos_profile2); - }); - EXPECT_NE(qos_profile1, qos_profile2); -} - -TEST(test_rmw_participant_qos_profile_allocator, test_free_null) { - rmw_participant_qos_profile_free(NULL); - SUCCEED(); -} - -TEST(test_rmw_participant_qos_profile_allocator, test_free_allocated) { - rmw_participant_qos_profile_t * qos_profile = rmw_participant_qos_profile_allocate(); - rmw_participant_qos_profile_free(qos_profile); - SUCCEED(); -} From 171400892673a53c3ccd65567ece4afef5270195 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 25 Oct 2019 10:10:08 -0700 Subject: [PATCH 10/31] - Changed member variables from const type const to const type in rmw_topic_info_t - Change type of gid to const char * Signed-off-by: Jaison Titus --- rmw/include/rmw/types.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index 2d2d7392..1541c8d7 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -433,13 +433,13 @@ typedef struct RMW_PUBLIC_TYPE rmw_offered_deadline_missed_status_t typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t { /// Name of the node - const char * const node_name; + const char * node_name; /// Namespace of the node - const char * const node_namespace; + const char * node_namespace; /// The associated TopicType - const char * const topic_type; + const char * topic_type; /// The GID of the node - const rmw_gid_t * gid; + const char * gid; /// Qos profile of the node const rmw_qos_profile_t * qos_profile; } rmw_topic_info_t; From 583687e14cb3efe001957979146b59cf62ffee88 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Mon, 28 Oct 2019 18:09:32 -0700 Subject: [PATCH 11/31] - Added zero_init, check_zero, init_with_size and fini functions for rmw_topic_info_array_t - Added tests for the same Signed-off-by: Jaison Titus --- rmw/CMakeLists.txt | 1 + rmw/include/rmw/topic_info_array.h | 85 ++++++++++++++++++++++++++++ rmw/package.xml | 1 + rmw/src/topic_info_array.c | 89 ++++++++++++++++++++++++++++++ rmw/test/CMakeLists.txt | 11 ++++ rmw/test/test_topic_info_array.cpp | 63 +++++++++++++++++++++ 6 files changed, 250 insertions(+) create mode 100644 rmw/include/rmw/topic_info_array.h create mode 100644 rmw/src/topic_info_array.c create mode 100644 rmw/test/test_topic_info_array.cpp diff --git a/rmw/CMakeLists.txt b/rmw/CMakeLists.txt index 8aa54068..020676ad 100644 --- a/rmw/CMakeLists.txt +++ b/rmw/CMakeLists.txt @@ -39,6 +39,7 @@ set(rmw_sources "src/validate_full_topic_name.c" "src/validate_namespace.c" "src/validate_node_name.c" + "src/topic_info_array.c" ) set_source_files_properties(${rmw_sources} PROPERTIES LANGUAGE "C") add_library(${PROJECT_NAME} ${rmw_sources}) diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_info_array.h new file mode 100644 index 00000000..bd44333d --- /dev/null +++ b/rmw/include/rmw/topic_info_array.h @@ -0,0 +1,85 @@ +// 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_INFO_ARRAY_H_ +#define RMW__TOPIC_INFO_ARRAY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw/types.h" +#include "rmw/visibility_control.h" + +/// Return a rmw_topic_info_array_t struct with members initialized to `NULL`. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_info_array_t +rmw_get_zero_initialized_topic_info_array(void); + +/// Check that a rmw_topic_info_array_t struct is zero initialized. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array); + +/// Initialize the info_array member inside rmw_topic_info_array_t with the given size +/** + * The rmw_topic_info_array_t has a member variable info_array which is an array of + * type rmw_topic_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. + * + * \param[in] allocator the allocator to be used to allocate space + * \param[in] size the size of the array + * \param[out] topic_info_array the data structure to initialise + * \returns `RMW_RET_OK` on successful init, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any of the parameters are NULL, 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_info_array_init_with_size( + rcutils_allocator_t * allocator, + size_t size, + rmw_topic_info_array_t * topic_info_array); + +/// Finalize a rmw_topic_info_array_t object. +/** + * The info_array member variable inside of rmw_topic_info_array represents an array of + * rmw_topic_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[in] allocator the allocator used to allocate memory to the object + * \param[out] topic_info_array object to be finalized + * \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_info_array_fini( + rcutils_allocator_t * allocator, + rmw_topic_info_array_t * topic_info_array); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__TOPIC_INFO_ARRAY_H_ 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_info_array.c b/rmw/src/topic_info_array.c new file mode 100644 index 00000000..5ce70b24 --- /dev/null +++ b/rmw/src/topic_info_array.c @@ -0,0 +1,89 @@ +// 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_info_array.h" + +#include "rcutils/logging_macros.h" +#include "rmw/error_handling.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/types.h" + +rmw_topic_info_array_t +rmw_get_zero_initialized_topic_info_array(void) +{ + rmw_topic_info_array_t zero = {0}; + return zero; +} + +rmw_ret_t +rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array) +{ + if (!topic_info_array) { + RMW_SET_ERROR_MSG("topic_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (topic_info_array->count != 0 || topic_info_array->info_array != NULL) { + RMW_SET_ERROR_MSG("topic_info_array is not zeroed"); + return RMW_RET_INVALID_ARGUMENT; + } + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_info_array_init_with_size( + rcutils_allocator_t * allocator, + size_t size, + rmw_topic_info_array_t * topic_info_array) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (!topic_info_array) { + RMW_SET_ERROR_MSG("topic_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + topic_info_array->info_array = allocator->allocate(sizeof(*topic_info_array->info_array) * size, + allocator->state); + if (!topic_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_info_array_fini( + rcutils_allocator_t * allocator, + rmw_topic_info_array_t * topic_info_array) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!topic_info_array) { + RMW_SET_ERROR_MSG("topic_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!topic_info_array->info_array) { + RMW_SET_ERROR_MSG("invalid topic_info_array"); + return RMW_RET_INVALID_ARGUMENT; + } + allocator->deallocate(topic_info_array->info_array, allocator->state); + topic_info_array->info_array = NULL; + topic_info_array->count = 0; + return RMW_RET_OK; +} diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index c6a12710..a91e3f81 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,13 @@ if(TARGET test_validate_namespace) set_target_properties(test_validate_namespace PROPERTIES COMPILE_FLAGS "-std=c++14") endif() endif() + +ament_add_gmock(test_topic_info_array + test_topic_info_array.cpp + # Append the directory of librmw so it is found at test time. + APPEND_LIBRARY_DIRS "$" +) +if(TARGET test_topic_info_array) + target_link_libraries(test_topic_info_array ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools) +endif() diff --git a/rmw/test/test_topic_info_array.cpp b/rmw/test/test_topic_info_array.cpp new file mode 100644 index 00000000..7ac7b6af --- /dev/null +++ b/rmw/test/test_topic_info_array.cpp @@ -0,0 +1,63 @@ +// 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_info_array.h" +#include "rmw/types.h" + +TEST(test_topic_info_array, zero_initialize) { + rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); + EXPECT_EQ(arr.count, 0u); + EXPECT_FALSE(arr.info_array); +} + +TEST(test_topic_info_array, check_zero) { + rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); + EXPECT_EQ(rmw_topic_info_array_check_zero(&arr), RMW_RET_OK); + rmw_topic_info_array_t arr_count_not_zero = {1, nullptr}; + EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_count_not_zero), RMW_RET_INVALID_ARGUMENT); + rmw_topic_info_t topic_info; + rmw_topic_info_array_t arr_info_array_not_null = {1, &topic_info}; + EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_info_array_not_null), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_check_zero(nullptr), RMW_RET_INVALID_ARGUMENT); +} + +TEST(test_topic_info_array, check_init_with_size) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + rmw_ret_t fini_ret = rmw_topic_info_array_fini(&allocator, &arr); + EXPECT_EQ(fini_ret, RMW_RET_OK); + }); + EXPECT_EQ(rmw_topic_info_array_init_with_size(nullptr, 1, &arr), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_init_with_size(&allocator, 1, nullptr), RMW_RET_INVALID_ARGUMENT); + EXPECT_FALSE(arr.info_array); + rmw_ret_t ret = rmw_topic_info_array_init_with_size(&allocator, 5, &arr); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_TRUE(arr.info_array); +} + +TEST(test_topic_info_array, check_fini) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); + rmw_ret_t ret = rmw_topic_info_array_init_with_size(&allocator, 5, &arr); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_TRUE(arr.info_array); + ret = rmw_topic_info_array_fini(&allocator, &arr); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_FALSE(arr.info_array); +} From 91a5668c7662f112b8833a232455686e5d8b2c11 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Wed, 13 Nov 2019 12:13:41 -0800 Subject: [PATCH 12/31] - More informative comments - Setters for allocating memory and copying strings for gid, topic_type, node_name and node_namespace Signed-off-by: Jaison Titus --- rmw/CMakeLists.txt | 4 +- rmw/include/rmw/rmw.h | 28 +++++++++ rmw/include/rmw/topic_info_array.h | 94 +++++++++++++++++++++++++++++ rmw/src/topic_info_array.c | 95 ++++++++++++++++++++++++++++++ 4 files changed, 219 insertions(+), 2 deletions(-) diff --git a/rmw/CMakeLists.txt b/rmw/CMakeLists.txt index 020676ad..624b1e2b 100644 --- a/rmw/CMakeLists.txt +++ b/rmw/CMakeLists.txt @@ -32,14 +32,14 @@ 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_info_array.c" "src/validate_full_topic_name.c" "src/validate_namespace.c" "src/validate_node_name.c" - "src/topic_info_array.c" ) set_source_files_properties(${rmw_sources} PROPERTIES LANGUAGE "C") add_library(${PROJECT_NAME} ${rmw_sources}) diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index c81f9e62..ba02355b 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1120,6 +1120,20 @@ rmw_set_log_severity(rmw_log_severity_t severity); * mentioned at http://design.ros2.org/articles/topic_and_service_names.html * Non existent topic names are allowed. They 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_info_array_t. The `count` field inside the struct + * must be set to 0 and the `info_array` field inside the struct must be set to null. + * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. + * @see rmw_topic_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * @see rmw_topic_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. @@ -1152,6 +1166,20 @@ rmw_get_publishers_info_by_topic( * mentioned at http://design.ros2.org/articles/topic_and_service_names.html * Non existent topic names 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_info_array_t. The `count` field inside the struct + * must be set to 0 and the `info_array` field inside the struct must be set to null. + * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. + * @see rmw_topic_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * @see rmw_topic_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. diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_info_array.h index bd44333d..83380ee2 100644 --- a/rmw/include/rmw/topic_info_array.h +++ b/rmw/include/rmw/topic_info_array.h @@ -78,6 +78,100 @@ rmw_topic_info_array_fini( rcutils_allocator_t * allocator, rmw_topic_info_array_t * topic_info_array); +/// Set the gid in rmw_topic_info_t. +/** + * rmw_topic_info_t has a member gid of type const char *; + * this function allocates memory and copies the value of param passed to it. + * + * \param[in] gid the gid value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \returns `RMW_RET_OK` on successfully setting the gid, 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_info_set_gid( + rcutils_allocator_t * allocator, + const char * gid, + rmw_topic_info_t * topic_info); + +/// Set the topic_type in rmw_topic_info_t. +/** + * rmw_topic_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[in] topic_type the topic_type value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_topic_type( + rcutils_allocator_t * allocator, + const char * topic_type, + rmw_topic_info_t * topic_info); + +/// Set the node_name in rmw_topic_info_t. +/** + * rmw_topic_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[in] node_name the node_name value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_node_name( + rcutils_allocator_t * allocator, + const char * node_name, + rmw_topic_info_t * topic_info); + +/// Set the node_namespace in rmw_topic_info_t. +/** + * rmw_topic_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[in] node_namespace the node_namespace value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_node_namespace( + rcutils_allocator_t * allocator, + const char * node_namespace, + rmw_topic_info_t * topic_info); + +/// Set the qos_profile in rmw_topic_info_t. +/** + * rmw_topic_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[in] qos_profile the qos_profile to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_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_info_set_qos_profile( + rmw_qos_profile_t * qos_profile, + rmw_topic_info_t * topic_info); + #ifdef __cplusplus } #endif diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index 5ce70b24..0758731d 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -82,8 +82,103 @@ rmw_topic_info_array_fini( RMW_SET_ERROR_MSG("invalid topic_info_array"); return RMW_RET_INVALID_ARGUMENT; } + // free all const char * inside the topic_info_t + for (size_t i = 0u; i < topic_info_array->count; i++) { + allocator->deallocate(topic_info_array->info_array[i].gid, allocator->state); + allocator->deallocate(topic_info_array->info_array[i].topic_type, allocator->state); + allocator->deallocate(topic_info_array->info_array[i].node_name, allocator->state); + allocator->deallocate(topic_info_array->info_array[i].node_namespace, allocator->state); + } + allocator->deallocate(topic_info_array->info_array, allocator->state); topic_info_array->info_array = NULL; topic_info_array->count = 0; return RMW_RET_OK; } + + +rmw_ret_t +_rmw_topic_info_copy_str( + rcutils_allocator_t * allocator, + const char * str, + const char ** topic_info_str) +{ + if (!str) { + RMW_SET_ERROR_MSG("str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!topic_info_str) { + RMW_SET_ERROR_MSG("topic_info_str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + char * temp_str = allocator->allocate(strlen(str) + 1, allocator->state); + strcpy(temp_str, str); + *topic_info_str = temp_str; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_info_set_gid( + rcutils_allocator_t * allocator, + const char * gid, + rmw_topic_info_t * topic_info) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(allocator, gid, &topic_info->gid); +} + +rmw_ret_t +rmw_topic_info_set_topic_type( + rcutils_allocator_t * allocator, + const char * topic_type, + rmw_topic_info_t * topic_info) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(allocator, topic_type, &topic_info->topic_type); +} + +rmw_ret_t +rmw_topic_info_set_node_name( + rcutils_allocator_t * allocator, + const char * node_name, + rmw_topic_info_t * topic_info) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(allocator, node_name, &topic_info->node_name); +} + +rmw_ret_t +rmw_topic_info_set_node_namespace( + rcutils_allocator_t * allocator, + const char * node_namespace, + rmw_topic_info_t * topic_info) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(allocator, node_namespace, &topic_info->node_namespace); +} + +rmw_ret_t +rmw_topic_info_set_qos_profile( + rmw_qos_profile_t * qos_profile, + rmw_topic_info_t * topic_info) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + topic_info->qos_profile = qos_profile; + return RMW_RET_OK; +} From 091988cc07e82f55c63a556119b14a2f218c3bce Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 14 Nov 2019 14:02:24 -0800 Subject: [PATCH 13/31] - Fixed warnings on deallocate by casting const char * to char * - No longer returning error if info_array is not allocated memory in fini Signed-off-by: Jaison Titus --- rmw/src/topic_info_array.c | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index 0758731d..a83d763b 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -78,16 +78,12 @@ rmw_topic_info_array_fini( return RMW_RET_INVALID_ARGUMENT; } - if (!topic_info_array->info_array) { - RMW_SET_ERROR_MSG("invalid topic_info_array"); - return RMW_RET_INVALID_ARGUMENT; - } // free all const char * inside the topic_info_t for (size_t i = 0u; i < topic_info_array->count; i++) { - allocator->deallocate(topic_info_array->info_array[i].gid, allocator->state); - allocator->deallocate(topic_info_array->info_array[i].topic_type, allocator->state); - allocator->deallocate(topic_info_array->info_array[i].node_name, allocator->state); - allocator->deallocate(topic_info_array->info_array[i].node_namespace, allocator->state); + allocator->deallocate((char *) topic_info_array->info_array[i].gid, allocator->state); + allocator->deallocate((char *) topic_info_array->info_array[i].topic_type, allocator->state); + allocator->deallocate((char *) topic_info_array->info_array[i].node_name, allocator->state); + allocator->deallocate((char *) topic_info_array->info_array[i].node_namespace, allocator->state); } allocator->deallocate(topic_info_array->info_array, allocator->state); From ad84fb3744891397b1b26af0a508bc8e4b3bc86f Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 15 Nov 2019 14:13:54 -0800 Subject: [PATCH 14/31] Fixed code formatting issues Signed-off-by: Jaison Titus --- rmw/src/topic_info_array.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index a83d763b..015fa381 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -83,7 +83,8 @@ rmw_topic_info_array_fini( allocator->deallocate((char *) topic_info_array->info_array[i].gid, allocator->state); allocator->deallocate((char *) topic_info_array->info_array[i].topic_type, allocator->state); allocator->deallocate((char *) topic_info_array->info_array[i].node_name, allocator->state); - allocator->deallocate((char *) topic_info_array->info_array[i].node_namespace, allocator->state); + allocator->deallocate((char *) topic_info_array->info_array[i].node_namespace, + allocator->state); } allocator->deallocate(topic_info_array->info_array, allocator->state); From 9b22c98a8c28fd4407f7ea0073d840495cf3158a Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 15 Nov 2019 16:57:41 -0800 Subject: [PATCH 15/31] - Using strncpy instead of strcpy Signed-off-by: Jaison Titus --- rmw/src/topic_info_array.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index 015fa381..d27de954 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -109,8 +109,9 @@ _rmw_topic_info_copy_str( RMW_SET_ERROR_MSG("topic_info_str is null"); return RMW_RET_INVALID_ARGUMENT; } - char * temp_str = allocator->allocate(strlen(str) + 1, allocator->state); - strcpy(temp_str, str); + size_t size = strlen(str) + 1; + char * temp_str = allocator->allocate(size, allocator->state); + strncpy(temp_str, str, size); *topic_info_str = temp_str; return RMW_RET_OK; } From 383e4a6afa984b03d5196e462158e8857574e569 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 19 Nov 2019 18:02:50 -0800 Subject: [PATCH 16/31] Using memcpy instead of strncpy to prevent warnings on Windows Signed-off-by: Jaison Titus --- rmw/src/topic_info_array.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index d27de954..589db2e5 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -111,7 +111,7 @@ _rmw_topic_info_copy_str( } size_t size = strlen(str) + 1; char * temp_str = allocator->allocate(size, allocator->state); - strncpy(temp_str, str, size); + memcpy(temp_str, str, size); *topic_info_str = temp_str; return RMW_RET_OK; } From 5e0e27f0cdb517321dc52ad1bab35ff7ebfa22fb Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Fri, 22 Nov 2019 09:10:24 -0800 Subject: [PATCH 17/31] - Comment modification to be more explicit. - Changed topic_info* method signatures - Moved topic_info_set* functions to a different file - Tests for topic_info_set* functions - Changed type of gid in rmw_topic_info_t to uint8_t[] - Changed type of qos_profile in rmw_topic_info_t from pointer to storing the structure. Signed-off-by: Jaison Titus --- rmw/CMakeLists.txt | 1 + rmw/include/rmw/rmw.h | 4 +- rmw/include/rmw/topic_info.h | 124 ++++++++++++++++++++++++ rmw/include/rmw/topic_info_array.h | 102 +------------------- rmw/include/rmw/types.h | 4 +- rmw/src/topic_info.c | 125 ++++++++++++++++++++++++ rmw/src/topic_info_array.c | 99 +------------------ rmw/test/CMakeLists.txt | 10 ++ rmw/test/test_topic_info.cpp | 148 +++++++++++++++++++++++++++++ rmw/test/test_topic_info_array.cpp | 14 +-- 10 files changed, 427 insertions(+), 204 deletions(-) create mode 100644 rmw/include/rmw/topic_info.h create mode 100644 rmw/src/topic_info.c create mode 100644 rmw/test/test_topic_info.cpp diff --git a/rmw/CMakeLists.txt b/rmw/CMakeLists.txt index 624b1e2b..7c0221a0 100644 --- a/rmw/CMakeLists.txt +++ b/rmw/CMakeLists.txt @@ -37,6 +37,7 @@ set(rmw_sources "src/sanity_checks.c" "src/subscription_options.c" "src/topic_info_array.c" + "src/topic_info.c" "src/validate_full_topic_name.c" "src/validate_namespace.c" "src/validate_node_name.c" diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index ba02355b..83051af0 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -1112,7 +1112,7 @@ rmw_set_log_severity(rmw_log_severity_t severity); /// Retrieves the information for all publishers to a given topic. /** * The retrieved information will contain the publisher's node name, node namespace, - * associated topic type, gid and qos profile. + * associated topic type, publisher gid and qos profile. * * The node parameter must not be `NULL` and must point to a valid node. * @@ -1158,7 +1158,7 @@ rmw_get_publishers_info_by_topic( /// Retrieves the information for all subscriptions to a given topic. /** * The retrieved information will contain the subscriptions's node name, node namespace, - * associated topic type, gid and qos profile. + * associated topic type, subscription gid and qos profile. * * The node parameter must not be `NULL` and must point to a valid node. * diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h new file mode 100644 index 00000000..fd016e40 --- /dev/null +++ b/rmw/include/rmw/topic_info.h @@ -0,0 +1,124 @@ +// 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_INFO_H_ +#define RMW__TOPIC_INFO_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw/types.h" +#include "rmw/visibility_control.h" + +/// Set the gid in rmw_topic_info_t. +/** + * Copies the values from gid into the gid member inside topic_info. + * + * \param[in] gid the gid value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \returns `RMW_RET_OK` on successfully setting the gid, 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_info_set_gid( + rmw_topic_info_t * topic_info, + const uint8_t gid[], + size_t size); + +/// Set the topic_type in rmw_topic_info_t. +/** + * rmw_topic_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[in] topic_type the topic_type value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_topic_type( + rmw_topic_info_t * topic_info, + const char * topic_type, + rcutils_allocator_t * allocator); + +/// Set the node_name in rmw_topic_info_t. +/** + * rmw_topic_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[in] node_name the node_name value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_node_name( + rmw_topic_info_t * topic_info, + const char * node_name, + rcutils_allocator_t * allocator); + +/// Set the node_namespace in rmw_topic_info_t. +/** + * rmw_topic_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[in] node_namespace the node_namespace value to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t + * \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_info_set_node_namespace( + rmw_topic_info_t * topic_info, + const char * node_namespace, + rcutils_allocator_t * allocator); + +/// Set the qos_profile in rmw_topic_info_t. +/** + * rmw_topic_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[in] qos_profile the qos_profile to set in rmw_topic_info_t + * \param[out] topic_info pointer to an initialized instance of rmw_topic_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_info_set_qos_profile( + rmw_topic_info_t * topic_info, + rmw_qos_profile_t * qos_profile); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__TOPIC_INFO_H_ diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_info_array.h index 83380ee2..ab403215 100644 --- a/rmw/include/rmw/topic_info_array.h +++ b/rmw/include/rmw/topic_info_array.h @@ -54,9 +54,9 @@ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t rmw_topic_info_array_init_with_size( - rcutils_allocator_t * allocator, + rmw_topic_info_array_t * topic_info_array, size_t size, - rmw_topic_info_array_t * topic_info_array); + rcutils_allocator_t * allocator); /// Finalize a rmw_topic_info_array_t object. /** @@ -75,102 +75,8 @@ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t rmw_topic_info_array_fini( - rcutils_allocator_t * allocator, - rmw_topic_info_array_t * topic_info_array); - -/// Set the gid in rmw_topic_info_t. -/** - * rmw_topic_info_t has a member gid of type const char *; - * this function allocates memory and copies the value of param passed to it. - * - * \param[in] gid the gid value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t - * \returns `RMW_RET_OK` on successfully setting the gid, 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_info_set_gid( - rcutils_allocator_t * allocator, - const char * gid, - rmw_topic_info_t * topic_info); - -/// Set the topic_type in rmw_topic_info_t. -/** - * rmw_topic_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[in] topic_type the topic_type value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t - * \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_info_set_topic_type( - rcutils_allocator_t * allocator, - const char * topic_type, - rmw_topic_info_t * topic_info); - -/// Set the node_name in rmw_topic_info_t. -/** - * rmw_topic_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[in] node_name the node_name value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t - * \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_info_set_node_name( - rcutils_allocator_t * allocator, - const char * node_name, - rmw_topic_info_t * topic_info); - -/// Set the node_namespace in rmw_topic_info_t. -/** - * rmw_topic_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[in] node_namespace the node_namespace value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t - * \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_info_set_node_namespace( - rcutils_allocator_t * allocator, - const char * node_namespace, - rmw_topic_info_t * topic_info); - -/// Set the qos_profile in rmw_topic_info_t. -/** - * rmw_topic_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[in] qos_profile the qos_profile to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_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_info_set_qos_profile( - rmw_qos_profile_t * qos_profile, - rmw_topic_info_t * topic_info); + rmw_topic_info_array_t * topic_info_array, + rcutils_allocator_t * allocator); #ifdef __cplusplus } diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index 1541c8d7..fc9be31b 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -439,9 +439,9 @@ typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t /// The associated TopicType const char * topic_type; /// The GID of the node - const char * gid; + uint8_t gid[RMW_GID_STORAGE_SIZE]; /// Qos profile of the node - const rmw_qos_profile_t * qos_profile; + rmw_qos_profile_t qos_profile; } rmw_topic_info_t; /// Array of rmw_topic_info_t diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c new file mode 100644 index 00000000..fbfd63ad --- /dev/null +++ b/rmw/src/topic_info.c @@ -0,0 +1,125 @@ +// 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_info.h" + +#include "rmw/error_handling.h" +#include "rmw/types.h" + +rmw_ret_t +_rmw_topic_info_copy_str( + const char ** topic_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_info_str) { + RMW_SET_ERROR_MSG("topic_info_str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + size_t size = strlen(str) + 1; + char * temp_str = allocator->allocate(size, allocator->state); + memcpy(temp_str, str, size); + *topic_info_str = temp_str; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_info_set_topic_type( + rmw_topic_info_t * topic_info, + const char * topic_type, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(&topic_info->topic_type, topic_type, allocator); +} + +rmw_ret_t +rmw_topic_info_set_node_name( + rmw_topic_info_t * topic_info, + const char * node_name, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(&topic_info->node_name, node_name, allocator); +} + +rmw_ret_t +rmw_topic_info_set_node_namespace( + rmw_topic_info_t * topic_info, + const char * node_namespace, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_copy_str(&topic_info->node_namespace, node_namespace, allocator); +} + +rmw_ret_t +rmw_topic_info_set_qos_profile( + rmw_topic_info_t * topic_info, + rmw_qos_profile_t * qos_profile) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_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_info->qos_profile = *qos_profile; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_info_set_gid( + rmw_topic_info_t * topic_info, + const uint8_t gid[], + size_t size) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_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_info->gid, 0, RMW_GID_STORAGE_SIZE); + for (uint i = 0u; i < size; i++) { + topic_info->gid[i] = gid[i]; + } + return RMW_RET_OK; +} diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index 589db2e5..e7155782 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -14,9 +14,7 @@ #include "rmw/topic_info_array.h" -#include "rcutils/logging_macros.h" #include "rmw/error_handling.h" -#include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/types.h" rmw_topic_info_array_t @@ -42,9 +40,9 @@ rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array) rmw_ret_t rmw_topic_info_array_init_with_size( - rcutils_allocator_t * allocator, + rmw_topic_info_array_t * topic_info_array, size_t size, - rmw_topic_info_array_t * topic_info_array) + rcutils_allocator_t * allocator) { if (!allocator) { RMW_SET_ERROR_MSG("allocator is null"); @@ -65,8 +63,8 @@ rmw_topic_info_array_init_with_size( rmw_ret_t rmw_topic_info_array_fini( - rcutils_allocator_t * allocator, - rmw_topic_info_array_t * topic_info_array) + rmw_topic_info_array_t * topic_info_array, + rcutils_allocator_t * allocator) { if (!allocator) { RMW_SET_ERROR_MSG("allocator is null"); @@ -80,7 +78,6 @@ rmw_topic_info_array_fini( // free all const char * inside the topic_info_t for (size_t i = 0u; i < topic_info_array->count; i++) { - allocator->deallocate((char *) topic_info_array->info_array[i].gid, allocator->state); allocator->deallocate((char *) topic_info_array->info_array[i].topic_type, allocator->state); allocator->deallocate((char *) topic_info_array->info_array[i].node_name, allocator->state); allocator->deallocate((char *) topic_info_array->info_array[i].node_namespace, @@ -92,91 +89,3 @@ rmw_topic_info_array_fini( topic_info_array->count = 0; return RMW_RET_OK; } - - -rmw_ret_t -_rmw_topic_info_copy_str( - rcutils_allocator_t * allocator, - const char * str, - const char ** topic_info_str) -{ - if (!str) { - RMW_SET_ERROR_MSG("str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!topic_info_str) { - RMW_SET_ERROR_MSG("topic_info_str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - size_t size = strlen(str) + 1; - char * temp_str = allocator->allocate(size, allocator->state); - memcpy(temp_str, str, size); - *topic_info_str = temp_str; - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_set_gid( - rcutils_allocator_t * allocator, - const char * gid, - rmw_topic_info_t * topic_info) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(allocator, gid, &topic_info->gid); -} - -rmw_ret_t -rmw_topic_info_set_topic_type( - rcutils_allocator_t * allocator, - const char * topic_type, - rmw_topic_info_t * topic_info) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(allocator, topic_type, &topic_info->topic_type); -} - -rmw_ret_t -rmw_topic_info_set_node_name( - rcutils_allocator_t * allocator, - const char * node_name, - rmw_topic_info_t * topic_info) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(allocator, node_name, &topic_info->node_name); -} - -rmw_ret_t -rmw_topic_info_set_node_namespace( - rcutils_allocator_t * allocator, - const char * node_namespace, - rmw_topic_info_t * topic_info) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(allocator, node_namespace, &topic_info->node_namespace); -} - -rmw_ret_t -rmw_topic_info_set_qos_profile( - rmw_qos_profile_t * qos_profile, - rmw_topic_info_t * topic_info) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - topic_info->qos_profile = qos_profile; - return RMW_RET_OK; -} diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index a91e3f81..bd393d4c 100644 --- a/rmw/test/CMakeLists.txt +++ b/rmw/test/CMakeLists.txt @@ -52,3 +52,13 @@ if(TARGET test_topic_info_array) target_link_libraries(test_topic_info_array ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools) endif() + +ament_add_gmock(test_topic_info + test_topic_info.cpp + # Append the directory of librmw so it is found at test time. + APPEND_LIBRARY_DIRS "$" +) +if(TARGET test_topic_info) + target_link_libraries(test_topic_info ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools) +endif() diff --git a/rmw/test/test_topic_info.cpp b/rmw/test/test_topic_info.cpp new file mode 100644 index 00000000..6fed3cd4 --- /dev/null +++ b/rmw/test/test_topic_info.cpp @@ -0,0 +1,148 @@ +// 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_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_info, set_topic_type) { + rmw_topic_info_t topic_info; + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_topic_type"); + rmw_ret_t ret = rmw_topic_info_set_topic_type(&topic_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_info_set_topic_type(&topic_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_type"; + ret = rmw_topic_info_set_topic_type(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_info.topic_type), allocator.state); + }); + ret = rmw_topic_info_set_topic_type(&topic_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_info.topic_type, "test_topic_type") << "Topic Type value is not as expected"; +} + +TEST(test_topic_info, set_node_name) { + rmw_topic_info_t topic_info; + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_node_name"); + rmw_ret_t ret = rmw_topic_info_set_node_name(&topic_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_info_set_node_name(&topic_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_name"; + ret = rmw_topic_info_set_node_name(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_info.node_name), allocator.state); + }); + ret = rmw_topic_info_set_node_name(&topic_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_info.node_name, "test_node_name") << "Node name value is not as expected"; +} + +TEST(test_topic_info, set_node_namespace) { + rmw_topic_info_t topic_info; + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_node_namespace"); + rmw_ret_t ret = rmw_topic_info_set_node_namespace(&topic_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_info_set_node_namespace(&topic_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_namespace"; + ret = rmw_topic_info_set_node_namespace(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_info.node_namespace), allocator.state); + }); + ret = rmw_topic_info_set_node_namespace(&topic_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_info.node_namespace, "test_node_namespace") << "Node namespace value is" + "not as expected"; +} + +TEST(test_topic_info, set_gid) { + rmw_topic_info_t topic_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_info_set_gid(nullptr, gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + ret = rmw_topic_info_set_gid(&topic_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_info_set_gid(&topic_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++) { + printf("gid[%d]: %d and topic_gid[%d]: %d", i, gid[i], i, topic_info.gid[i]); + EXPECT_EQ(topic_info.gid[i], gid[i]) << "Gid value is not as expected"; + } +} + +TEST(test_topic_info, set_qos_profile) { + rmw_topic_info_t topic_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_info_set_qos_profile(nullptr, &qos_profile); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + ret = rmw_topic_info_set_qos_profile(&topic_info, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null qos_profile"; + ret = rmw_topic_info_set_qos_profile(&topic_info, &qos_profile); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + + EXPECT_EQ(topic_info.qos_profile.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST) << "Unequal history"; + EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Unequal depth"; + EXPECT_EQ(topic_info.qos_profile.reliability, + RMW_QOS_POLICY_RELIABILITY_RELIABLE) << "Unequal reliability"; + EXPECT_EQ(topic_info.qos_profile.durability, + RMW_QOS_POLICY_DURABILITY_VOLATILE) << "Unequal durability"; + EXPECT_EQ(topic_info.qos_profile.deadline.sec, 1u) << "Unequal deadline sec"; + EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Unequal deadline nsec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 2u) << "Unequal lifespan sec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Unequal lifespan nsec"; + EXPECT_EQ(topic_info.qos_profile.liveliness, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) << "Unequal liveliness"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, + 3u) << "Unequal liveliness lease duration sec"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, + 0u) << "Unequal liveliness lease duration nsec"; + EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, + false) << "Unequal avoid namespace conventions"; +} diff --git a/rmw/test/test_topic_info_array.cpp b/rmw/test/test_topic_info_array.cpp index 7ac7b6af..ec59f348 100644 --- a/rmw/test/test_topic_info_array.cpp +++ b/rmw/test/test_topic_info_array.cpp @@ -31,7 +31,7 @@ TEST(test_topic_info_array, check_zero) { rmw_topic_info_array_t arr_count_not_zero = {1, nullptr}; EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_count_not_zero), RMW_RET_INVALID_ARGUMENT); rmw_topic_info_t topic_info; - rmw_topic_info_array_t arr_info_array_not_null = {1, &topic_info}; + rmw_topic_info_array_t arr_info_array_not_null = {0, &topic_info}; EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_info_array_not_null), RMW_RET_INVALID_ARGUMENT); EXPECT_EQ(rmw_topic_info_array_check_zero(nullptr), RMW_RET_INVALID_ARGUMENT); } @@ -40,13 +40,13 @@ TEST(test_topic_info_array, check_init_with_size) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - rmw_ret_t fini_ret = rmw_topic_info_array_fini(&allocator, &arr); + rmw_ret_t fini_ret = rmw_topic_info_array_fini(&arr, &allocator); EXPECT_EQ(fini_ret, RMW_RET_OK); }); - EXPECT_EQ(rmw_topic_info_array_init_with_size(nullptr, 1, &arr), RMW_RET_INVALID_ARGUMENT); - EXPECT_EQ(rmw_topic_info_array_init_with_size(&allocator, 1, nullptr), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_init_with_size(&arr, 1, nullptr), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_init_with_size(nullptr, 1, &allocator), RMW_RET_INVALID_ARGUMENT); EXPECT_FALSE(arr.info_array); - rmw_ret_t ret = rmw_topic_info_array_init_with_size(&allocator, 5, &arr); + rmw_ret_t ret = rmw_topic_info_array_init_with_size(&arr, 5, &allocator); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_TRUE(arr.info_array); } @@ -54,10 +54,10 @@ TEST(test_topic_info_array, check_init_with_size) { TEST(test_topic_info_array, check_fini) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); - rmw_ret_t ret = rmw_topic_info_array_init_with_size(&allocator, 5, &arr); + rmw_ret_t ret = rmw_topic_info_array_init_with_size(&arr, 5, &allocator); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_TRUE(arr.info_array); - ret = rmw_topic_info_array_fini(&allocator, &arr); + ret = rmw_topic_info_array_fini(&arr, &allocator); EXPECT_EQ(ret, RMW_RET_OK); EXPECT_FALSE(arr.info_array); } From 1d9e1feeb5c1ee807291900972a6c975bcab6ef1 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 26 Nov 2019 10:37:18 -0800 Subject: [PATCH 18/31] - Fixed function comments - Moved get_*_info_by_topic functions to a new file Signed-off-by: Jaison Titus --- rmw/include/rmw/get_topic_info.h | 122 +++++++++++++++++++++++++++++ rmw/include/rmw/rmw.h | 93 +--------------------- rmw/include/rmw/topic_info.h | 113 ++++++++++++++++++++------ rmw/include/rmw/topic_info_array.h | 20 ++++- rmw/include/rmw/types.h | 25 ------ rmw/src/topic_info.c | 56 ++++++++++++- rmw/src/topic_info_array.c | 18 +++-- 7 files changed, 298 insertions(+), 149 deletions(-) create mode 100644 rmw/include/rmw/get_topic_info.h diff --git a/rmw/include/rmw/get_topic_info.h b/rmw/include/rmw/get_topic_info.h new file mode 100644 index 00000000..760d9fff --- /dev/null +++ b/rmw/include/rmw/get_topic_info.h @@ -0,0 +1,122 @@ +// 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_INFO_H_ +#define RMW__GET_TOPIC_INFO_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rmw/topic_info_array.h" +#include "rmw/visibility_control.h" + +/// Retrieves 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 + * Non existent topic names are allowed. They 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_info_array_t. The `count` field inside the struct + * must be set to 0 and the `info_array` field inside the struct must be set to null. + * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. + * @see rmw_topic_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * @see rmw_topic_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_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_info_array_t * publishers_info); + +/// Retrieves 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 + * Non existent topic names 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_info_array_t. The `count` field inside the struct + * must be set to 0 and the `info_array` field inside the struct must be set to null. + * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. + * @see rmw_topic_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * @see rmw_topic_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_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_info_array_t * subscriptions_info); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__GET_TOPIC_INFO_H_ diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index 83051af0..059b3203 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -56,6 +56,7 @@ * - rmw/names_and_types.h * - rmw/get_topic_names_and_types.h * - rmw/get_service_names_and_types.h + * - rmw/get_topic_info.h * * Further still there are some useful abstractions and utilities: * @@ -1109,98 +1110,6 @@ RMW_WARN_UNUSED rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity); -/// Retrieves 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 - * Non existent topic names are allowed. They 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_info_array_t. The `count` field inside the struct - * must be set to 0 and the `info_array` field inside the struct must be set to null. - * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. - * @see rmw_topic_info_set_node_name and the likes. - * However, it is the responsibility of the caller to - * reclaim any allocated resources to `publishers_info` to avoid leaking memory. - * @see rmw_topic_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_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_info_array_t * publishers_info); - -/// Retrieves 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 - * Non existent topic names 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_info_array_t. The `count` field inside the struct - * must be set to 0 and the `info_array` field inside the struct must be set to null. - * @see rmw_get_zero_initialized_topic_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_info_t will be allocated memory and assigned a copied value. - * @see rmw_topic_info_set_node_name and the likes. - * However, it is the responsibility of the caller to - * reclaim any allocated resources to `publishers_info` to avoid leaking memory. - * @see rmw_topic_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_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_info_array_t * subscriptions_info); - #ifdef __cplusplus } #endif diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index fd016e40..33cf6f86 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -24,31 +24,30 @@ extern "C" #include "rmw/types.h" #include "rmw/visibility_control.h" -/// Set the gid in rmw_topic_info_t. -/** - * Copies the values from gid into the gid member inside topic_info. - * - * \param[in] gid the gid value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_info_t - * \returns `RMW_RET_OK` on successfully setting the gid, 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_info_set_gid( - rmw_topic_info_t * topic_info, - const uint8_t gid[], - size_t size); +/// 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_info_t +{ + /// Name of the node + const char * node_name; + /// Namespace of the node + const char * node_namespace; + /// The associated TopicType + const char * topic_type; + /// The GID of the node + uint8_t gid[RMW_GID_STORAGE_SIZE]; + /// Qos profile of the node + rmw_qos_profile_t qos_profile; +} rmw_topic_info_t; /// Set the topic_type in rmw_topic_info_t. /** * rmw_topic_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_info pointer to an initialized instance of rmw_topic_info_t * \param[in] topic_type the topic_type value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_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. @@ -66,8 +65,9 @@ rmw_topic_info_set_topic_type( * rmw_topic_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_info pointer to an initialized instance of rmw_topic_info_t * \param[in] node_name the node_name value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_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. @@ -85,8 +85,9 @@ rmw_topic_info_set_node_name( * rmw_topic_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_info pointer to an initialized instance of rmw_topic_info_t * \param[in] node_namespace the node_namespace value to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_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. @@ -104,8 +105,8 @@ rmw_topic_info_set_node_namespace( * rmw_topic_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_info pointer to an initialized instance of rmw_topic_info_t * \param[in] qos_profile the qos_profile to set in rmw_topic_info_t - * \param[out] topic_info pointer to an initialized instance of rmw_topic_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. @@ -115,7 +116,75 @@ RMW_WARN_UNUSED rmw_ret_t rmw_topic_info_set_qos_profile( rmw_topic_info_t * topic_info, - rmw_qos_profile_t * qos_profile); + const rmw_qos_profile_t * qos_profile); + +/// Set the gid in rmw_topic_info_t. +/** + * Copies the values from gid into the gid member inside topic_info. + * + * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t + * \param[in] gid the gid value to set in rmw_topic_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_info_set_gid( + rmw_topic_info_t * topic_info, + const uint8_t gid[], + size_t size); + +/// Free the memory which was allocated for topic_info.node_name +/** + * + * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t + * \param[in] allocator the allocator that was used to allocate memory + * \returns `RMW_RET_OK` on successfully freeing the 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_info_fini_node_name( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator); + +/// Free the memory which was allocated for topic_info.node_namespace +/** + * + * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t + * \param[in] allocator the allocator that was used to allocate memory + * \returns `RMW_RET_OK` on successfully freeing the 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_info_fini_node_namespace( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator); + +/// Free the memory which was allocated for topic_info.topic_type +/** + * + * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t + * \param[in] allocator the allocator that was used to allocate memory + * \returns `RMW_RET_OK` on successfully freeing the 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_info_fini_topic_type( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator); #ifdef __cplusplus } diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_info_array.h index ab403215..e7df7f41 100644 --- a/rmw/include/rmw/topic_info_array.h +++ b/rmw/include/rmw/topic_info_array.h @@ -21,9 +21,18 @@ extern "C" #endif #include "rcutils/allocator.h" -#include "rmw/types.h" +#include "rmw/topic_info.h" #include "rmw/visibility_control.h" +/// Array of rmw_topic_info_t +typedef struct RMW_PUBLIC_TYPE rmw_topic_info_array_t +{ + /// Size of the array. + size_t count; + /// Pointer representing an array of rmw_topic_info_t + rmw_topic_info_t * info_array; +} rmw_topic_info_array_t; + /// Return a rmw_topic_info_array_t struct with members initialized to `NULL`. RMW_PUBLIC RMW_WARN_UNUSED @@ -42,11 +51,14 @@ rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array); * type rmw_topic_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. * - * \param[in] allocator the allocator to be used to allocate space + * topic_info_array must be zero initialized before being passed into this function. + * + * \param[inout] topic_info_array the data structure to initialise * \param[in] size the size of the array - * \param[out] topic_info_array the data structure to initialise + * \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_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. */ @@ -65,8 +77,8 @@ rmw_topic_info_array_init_with_size( * This function reclaims any allocated resources within the object and also sets the value of count * to 0. * + * \param[inout] topic_info_array object to be finalized * \param[in] allocator the allocator used to allocate memory to the object - * \param[out] topic_info_array object to be finalized * \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. diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index fc9be31b..c68e00f7 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -428,31 +428,6 @@ typedef struct RMW_PUBLIC_TYPE rmw_offered_deadline_missed_status_t int32_t total_count_change; } rmw_offered_deadline_missed_status_t; -/// 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_info_t -{ - /// Name of the node - const char * node_name; - /// Namespace of the node - const char * node_namespace; - /// The associated TopicType - const char * topic_type; - /// The GID of the node - uint8_t gid[RMW_GID_STORAGE_SIZE]; - /// Qos profile of the node - rmw_qos_profile_t qos_profile; -} rmw_topic_info_t; - -/// Array of rmw_topic_info_t -typedef struct RMW_PUBLIC_TYPE rmw_topic_info_array_t -{ - /// Size of the array. - size_t count; - /// Pointer representing an array of rmw_topic_info_t - rmw_topic_info_t * info_array; -} rmw_topic_info_array_t; - #ifdef __cplusplus } #endif diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index fbfd63ad..fba09c05 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -87,7 +87,7 @@ rmw_topic_info_set_node_namespace( rmw_ret_t rmw_topic_info_set_qos_profile( rmw_topic_info_t * topic_info, - rmw_qos_profile_t * qos_profile) + const rmw_qos_profile_t * qos_profile) { if (!topic_info) { RMW_SET_ERROR_MSG("topic_info is null"); @@ -123,3 +123,57 @@ rmw_topic_info_set_gid( } return RMW_RET_OK; } + +rmw_ret_t +_rmw_topic_info_fini_str( + const char ** topic_info_str, + rcutils_allocator_t * allocator) +{ + if (!topic_info_str) { + RMW_SET_ERROR_MSG("topic_info_str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + allocator->deallocate((char *) *topic_info_str, allocator->state); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_info_fini_node_name( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->node_name, allocator); +} + +rmw_ret_t +rmw_topic_info_fini_node_namespace( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->node_namespace, allocator); +} + +rmw_ret_t +rmw_topic_info_fini_topic_type( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->topic_type, allocator); +} diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index e7155782..cf02d51d 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -13,7 +13,6 @@ // limitations under the License. #include "rmw/topic_info_array.h" - #include "rmw/error_handling.h" #include "rmw/types.h" @@ -76,12 +75,21 @@ rmw_topic_info_array_fini( return RMW_RET_INVALID_ARGUMENT; } + rmw_ret_t ret; // free all const char * inside the topic_info_t for (size_t i = 0u; i < topic_info_array->count; i++) { - allocator->deallocate((char *) topic_info_array->info_array[i].topic_type, allocator->state); - allocator->deallocate((char *) topic_info_array->info_array[i].node_name, allocator->state); - allocator->deallocate((char *) topic_info_array->info_array[i].node_namespace, - allocator->state); + ret = rmw_topic_info_fini_node_name(&topic_info_array->info_array[i], allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = rmw_topic_info_fini_node_namespace(&topic_info_array->info_array[i], allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = rmw_topic_info_fini_topic_type(&topic_info_array->info_array[i], allocator); + if (ret != RMW_RET_OK) { + return ret; + } } allocator->deallocate(topic_info_array->info_array, allocator->state); From cbd91b3dd19c163cd41e27150593d66ae390efea Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 26 Nov 2019 19:25:22 -0800 Subject: [PATCH 19/31] Using memcpy in topic_info_set_gid Signed-off-by: Jaison Titus --- rmw/src/topic_info.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index fba09c05..7aa7f553 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -118,9 +118,7 @@ rmw_topic_info_set_gid( return RMW_RET_INVALID_ARGUMENT; } memset(&topic_info->gid, 0, RMW_GID_STORAGE_SIZE); - for (uint i = 0u; i < size; i++) { - topic_info->gid[i] = gid[i]; - } + memcpy(topic_info->gid, gid, size); return RMW_RET_OK; } From e24e1f407e879ff02dc93dfb84f1d00437b25d95 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Wed, 27 Nov 2019 11:46:55 -0800 Subject: [PATCH 20/31] Added init and fini functions for rmw_topic_info_t and tests for the same. Signed-off-by: Jaison Titus --- rmw/include/rmw/topic_info.h | 25 +++++++++++ rmw/src/topic_info.c | 27 ++++++++++++ rmw/test/test_topic_info.cpp | 85 +++++++++++++++++++++++++++++++++++- 3 files changed, 136 insertions(+), 1 deletion(-) diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index 33cf6f86..d1d090a2 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -40,6 +40,31 @@ typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t rmw_qos_profile_t qos_profile; } rmw_topic_info_t; +/// Return a rmw_topic_info_t struct with members initialized to `NULL`. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_info_t +rmw_get_zero_initialized_topic_info(void); + +/// Finalize a rmw_topic_info_t object. +/** + * The rmw_topic_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_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_info_fini( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator); + /// Set the topic_type in rmw_topic_info_t. /** * rmw_topic_info_t has a member topic_type of type const char *; diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index 7aa7f553..8bb9199f 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -17,6 +17,33 @@ #include "rmw/error_handling.h" #include "rmw/types.h" +rmw_topic_info_t +rmw_get_zero_initialized_topic_info(void) +{ + rmw_topic_info_t zero = {0}; + return zero; +} + +rmw_ret_t +rmw_topic_info_fini( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info) { + RMW_SET_ERROR_MSG("topic_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + allocator->deallocate((char *) topic_info->node_name, allocator->state); + allocator->deallocate((char *) topic_info->node_namespace, allocator->state); + allocator->deallocate((char *) topic_info->topic_type, allocator->state); + *topic_info = rmw_get_zero_initialized_topic_info(); + return RMW_RET_OK; +} + rmw_ret_t _rmw_topic_info_copy_str( const char ** topic_info_str, diff --git a/rmw/test/test_topic_info.cpp b/rmw/test/test_topic_info.cpp index 6fed3cd4..993b8717 100644 --- a/rmw/test/test_topic_info.cpp +++ b/rmw/test/test_topic_info.cpp @@ -102,7 +102,6 @@ TEST(test_topic_info, set_gid) { ret = rmw_topic_info_set_gid(&topic_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++) { - printf("gid[%d]: %d and topic_gid[%d]: %d", i, gid[i], i, topic_info.gid[i]); EXPECT_EQ(topic_info.gid[i], gid[i]) << "Gid value is not as expected"; } } @@ -146,3 +145,87 @@ TEST(test_topic_info, set_qos_profile) { EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, false) << "Unequal avoid namespace conventions"; } + +TEST(test_topic_info, zero_init) { + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); + EXPECT_FALSE(topic_info.node_name); + EXPECT_FALSE(topic_info.node_namespace); + EXPECT_FALSE(topic_info.topic_type); + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + EXPECT_EQ(topic_info.gid[i], 0) << "Gid value should be 0"; + } + EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; + EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; + EXPECT_EQ(topic_info.qos_profile.reliability, 0) << "Non-zero reliability"; + EXPECT_EQ(topic_info.qos_profile.durability, 0) << "Non-zero durability"; + EXPECT_EQ(topic_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; + EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; + EXPECT_EQ(topic_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, + 0u) << "Non-zero liveliness lease duration sec"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, + 0u) << "Non-zero liveliness lease duration nsec"; + EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, + false) << "Non-zero avoid namespace conventions"; +} + +TEST(test_topic_info, fini) { + rmw_ret_t ret; + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_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; + ret = rmw_topic_info_set_qos_profile(&topic_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_info_set_gid(&topic_info, gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid gid arguments"; + ret = rmw_topic_info_set_node_namespace(&topic_info, "namespace", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_namespace arguments"; + ret = rmw_topic_info_set_node_name(&topic_info, "name", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_name arguments"; + ret = rmw_topic_info_set_topic_type(&topic_info, "type", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid topic_type arguments"; + ret = rmw_topic_info_fini(&topic_info, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_info_fini(nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; + + ret = rmw_topic_info_fini(&topic_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_info.node_name); + EXPECT_FALSE(topic_info.node_namespace); + EXPECT_FALSE(topic_info.topic_type); + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + EXPECT_EQ(topic_info.gid[i], 0) << "Gid value should be 0"; + } + EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; + EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; + EXPECT_EQ(topic_info.qos_profile.reliability, 0) << "Non-zero reliability"; + EXPECT_EQ(topic_info.qos_profile.durability, 0) << "Non-zero durability"; + EXPECT_EQ(topic_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; + EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; + EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; + EXPECT_EQ(topic_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, + 0u) << "Non-zero liveliness lease duration sec"; + EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, + 0u) << "Non-zero liveliness lease duration nsec"; + EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, + false) << "Non-zero avoid namespace conventions"; +} From 272a2dc87043c53cf92b6ede631d3c81c85395a0 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 10 Dec 2019 17:47:28 -0800 Subject: [PATCH 21/31] change c-strings under topic_info to be non-const Signed-off-by: Miaofei --- rmw/include/rmw/topic_info.h | 6 +++--- rmw/src/topic_info.c | 15 +++++++++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index d1d090a2..66d9ffdf 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -29,11 +29,11 @@ extern "C" typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t { /// Name of the node - const char * node_name; + char * node_name; /// Namespace of the node - const char * node_namespace; + char * node_namespace; /// The associated TopicType - const char * topic_type; + char * topic_type; /// The GID of the node uint8_t gid[RMW_GID_STORAGE_SIZE]; /// Qos profile of the node diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index 8bb9199f..0431c1e3 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -37,16 +37,16 @@ rmw_topic_info_fini( RMW_SET_ERROR_MSG("allocator is null"); return RMW_RET_INVALID_ARGUMENT; } - allocator->deallocate((char *) topic_info->node_name, allocator->state); - allocator->deallocate((char *) topic_info->node_namespace, allocator->state); - allocator->deallocate((char *) topic_info->topic_type, allocator->state); + allocator->deallocate(topic_info->node_name, allocator->state); + allocator->deallocate(topic_info->node_namespace, allocator->state); + allocator->deallocate(topic_info->topic_type, allocator->state); *topic_info = rmw_get_zero_initialized_topic_info(); return RMW_RET_OK; } rmw_ret_t _rmw_topic_info_copy_str( - const char ** topic_info_str, + char ** topic_info_str, const char * str, rcutils_allocator_t * allocator) { @@ -151,7 +151,7 @@ rmw_topic_info_set_gid( rmw_ret_t _rmw_topic_info_fini_str( - const char ** topic_info_str, + char ** topic_info_str, rcutils_allocator_t * allocator) { if (!topic_info_str) { @@ -163,7 +163,10 @@ _rmw_topic_info_fini_str( RMW_SET_ERROR_MSG("allocator is null"); return RMW_RET_INVALID_ARGUMENT; } - allocator->deallocate((char *) *topic_info_str, allocator->state); + + allocator->deallocate(*topic_info_str, allocator->state); + *topic_info_str = NULL; + return RMW_RET_OK; } From c00ad69311e0c2a221db975e927de1dba7e75b90 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 11 Dec 2019 16:24:19 -0800 Subject: [PATCH 22/31] revert the change that removed const qualifiers Signed-off-by: Miaofei --- rmw/include/rmw/topic_info.h | 6 +++--- rmw/src/topic_info.c | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index 66d9ffdf..d1d090a2 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -29,11 +29,11 @@ extern "C" typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t { /// Name of the node - char * node_name; + const char * node_name; /// Namespace of the node - char * node_namespace; + const char * node_namespace; /// The associated TopicType - char * topic_type; + const char * topic_type; /// The GID of the node uint8_t gid[RMW_GID_STORAGE_SIZE]; /// Qos profile of the node diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index 0431c1e3..5b53e106 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -37,16 +37,16 @@ rmw_topic_info_fini( RMW_SET_ERROR_MSG("allocator is null"); return RMW_RET_INVALID_ARGUMENT; } - allocator->deallocate(topic_info->node_name, allocator->state); - allocator->deallocate(topic_info->node_namespace, allocator->state); - allocator->deallocate(topic_info->topic_type, allocator->state); + allocator->deallocate((char *) topic_info->node_name, allocator->state); + allocator->deallocate((char *) topic_info->node_namespace, allocator->state); + allocator->deallocate((char *) topic_info->topic_type, allocator->state); *topic_info = rmw_get_zero_initialized_topic_info(); return RMW_RET_OK; } rmw_ret_t _rmw_topic_info_copy_str( - char ** topic_info_str, + const char ** topic_info_str, const char * str, rcutils_allocator_t * allocator) { @@ -151,7 +151,7 @@ rmw_topic_info_set_gid( rmw_ret_t _rmw_topic_info_fini_str( - char ** topic_info_str, + const char ** topic_info_str, rcutils_allocator_t * allocator) { if (!topic_info_str) { @@ -164,7 +164,7 @@ _rmw_topic_info_fini_str( return RMW_RET_INVALID_ARGUMENT; } - allocator->deallocate(*topic_info_str, allocator->state); + allocator->deallocate((char *) *topic_info_str, allocator->state); *topic_info_str = NULL; return RMW_RET_OK; From 3e8f8924858c1a8b4d63b8c36a90f14e9c438514 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 11 Dec 2019 17:01:44 -0800 Subject: [PATCH 23/31] address PR comments Signed-off-by: Miaofei --- rmw/include/rmw/get_topic_info.h | 18 ++++++++++++------ rmw/include/rmw/topic_info.h | 4 ++-- rmw/include/rmw/topic_info_array.h | 14 ++++++++++++-- rmw/src/topic_info.c | 2 +- rmw/src/topic_info_array.c | 2 +- rmw/test/test_topic_info.cpp | 13 ++++++------- rmw/test/test_topic_info_array.cpp | 4 ++-- 7 files changed, 36 insertions(+), 21 deletions(-) diff --git a/rmw/include/rmw/get_topic_info.h b/rmw/include/rmw/get_topic_info.h index 760d9fff..6d6e7149 100644 --- a/rmw/include/rmw/get_topic_info.h +++ b/rmw/include/rmw/get_topic_info.h @@ -32,7 +32,8 @@ extern "C" * * 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 - * Non existent topic names are allowed. They will return an empty array. + * Non existent topic names 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_info_array_t. The `count` field inside the struct @@ -42,8 +43,10 @@ extern "C" * 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_info_t will be allocated memory and assigned a copied value. - * @see rmw_topic_info_set_node_name and the likes. + * rmw_topic_info_t will be assigned a copied value on allocated memory. + * @see rmw_topic_info_set_topic_type + * @see rmw_topic_info_set_node_name + * @see rmw_topic_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_info_array_fini @@ -78,7 +81,8 @@ rmw_get_publishers_info_by_topic( * * 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 - * Non existent topic names are allowed. They will return an empty array. + * Non existent topic names 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_info_array_t. The `count` field inside the struct @@ -88,8 +92,10 @@ rmw_get_publishers_info_by_topic( * 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_info_t will be allocated memory and assigned a copied value. - * @see rmw_topic_info_set_node_name and the likes. + * rmw_topic_info_t will be assigned a copied value on allocated memory. + * @see rmw_topic_info_set_topic_type + * @see rmw_topic_info_set_node_name + * @see rmw_topic_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_info_array_fini diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index d1d090a2..6aec7fa4 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -24,7 +24,7 @@ extern "C" #include "rmw/types.h" #include "rmw/visibility_control.h" -/// A Structure that encapsulates the name, namespace, topic_type, gid and qos_profile +/// 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_info_t { @@ -32,7 +32,7 @@ typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t const char * node_name; /// Namespace of the node const char * node_namespace; - /// The associated TopicType + /// The associated topic type const char * topic_type; /// The GID of the node uint8_t gid[RMW_GID_STORAGE_SIZE]; diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_info_array.h index e7df7f41..b5f1530a 100644 --- a/rmw/include/rmw/topic_info_array.h +++ b/rmw/include/rmw/topic_info_array.h @@ -40,6 +40,14 @@ rmw_topic_info_array_t rmw_get_zero_initialized_topic_info_array(void); /// Check that a rmw_topic_info_array_t struct is zero initialized. +/** + * This function checks if the provided rmw_topic_info_array_t is zero initialized or not. + * + * \param[in] topic_info_array the data structure to be checked + * \returns `RMW_RET_OK` if topic_info_array is zero initialized + * \returns `RMW_RET_INVALID_ARGUMENT` if the parameter is NULL, or + * \returns `RMW_RET_ERROR` if topic_info_array is not zero initialized + */ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t @@ -48,7 +56,8 @@ rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array); /// Initialize the info_array member inside rmw_topic_info_array_t with the given size /** * The rmw_topic_info_array_t has a member variable info_array which is an array of - * type rmw_topic_info_t. This function allocates memory to this array to hold n elements, + * type rmw_topic_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_info_array must be zero initialized before being passed into this function. @@ -73,7 +82,8 @@ rmw_topic_info_array_init_with_size( /// Finalize a rmw_topic_info_array_t object. /** * The info_array member variable inside of rmw_topic_info_array represents an array of - * rmw_topic_info_t. When initializing this array, memory is allocated for it using the allocator. + * rmw_topic_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. * diff --git a/rmw/src/topic_info.c b/rmw/src/topic_info.c index 5b53e106..d01974e0 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -144,7 +144,7 @@ rmw_topic_info_set_gid( RMW_SET_ERROR_MSG("size is more than RMW_GID_STORAGE_SIZE"); return RMW_RET_INVALID_ARGUMENT; } - memset(&topic_info->gid, 0, RMW_GID_STORAGE_SIZE); + memset(topic_info->gid, 0, RMW_GID_STORAGE_SIZE); memcpy(topic_info->gid, gid, size); return RMW_RET_OK; } diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index cf02d51d..b5104940 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -32,7 +32,7 @@ rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array) } if (topic_info_array->count != 0 || topic_info_array->info_array != NULL) { RMW_SET_ERROR_MSG("topic_info_array is not zeroed"); - return RMW_RET_INVALID_ARGUMENT; + return RMW_RET_ERROR; } return RMW_RET_OK; } diff --git a/rmw/test/test_topic_info.cpp b/rmw/test/test_topic_info.cpp index 993b8717..1d04e107 100644 --- a/rmw/test/test_topic_info.cpp +++ b/rmw/test/test_topic_info.cpp @@ -28,7 +28,7 @@ char * get_mallocd_string(const char * s) } TEST(test_topic_info, set_topic_type) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rcutils_allocator_t allocator = rcutils_get_default_allocator(); char * val = get_mallocd_string("test_topic_type"); rmw_ret_t ret = rmw_topic_info_set_topic_type(&topic_info, val, nullptr); @@ -48,7 +48,7 @@ TEST(test_topic_info, set_topic_type) { } TEST(test_topic_info, set_node_name) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rcutils_allocator_t allocator = rcutils_get_default_allocator(); char * val = get_mallocd_string("test_node_name"); rmw_ret_t ret = rmw_topic_info_set_node_name(&topic_info, val, nullptr); @@ -68,7 +68,7 @@ TEST(test_topic_info, set_node_name) { } TEST(test_topic_info, set_node_namespace) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rcutils_allocator_t allocator = rcutils_get_default_allocator(); char * val = get_mallocd_string("test_node_namespace"); rmw_ret_t ret = rmw_topic_info_set_node_namespace(&topic_info, val, nullptr); @@ -89,7 +89,7 @@ TEST(test_topic_info, set_node_namespace) { } TEST(test_topic_info, set_gid) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); uint8_t gid[RMW_GID_STORAGE_SIZE]; for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { gid[i] = i; @@ -107,7 +107,7 @@ TEST(test_topic_info, set_gid) { } TEST(test_topic_info, set_qos_profile) { - rmw_topic_info_t topic_info; + rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rmw_qos_profile_t qos_profile; qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; qos_profile.depth = 0; @@ -172,7 +172,6 @@ TEST(test_topic_info, zero_init) { } TEST(test_topic_info, fini) { - rmw_ret_t ret; rcutils_allocator_t allocator = rcutils_get_default_allocator(); rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); rmw_qos_profile_t qos_profile; @@ -185,7 +184,7 @@ TEST(test_topic_info, fini) { qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; qos_profile.liveliness_lease_duration = {3, 0}; qos_profile.avoid_ros_namespace_conventions = false; - ret = rmw_topic_info_set_qos_profile(&topic_info, &qos_profile); + rmw_ret_t ret = rmw_topic_info_set_qos_profile(&topic_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++) { diff --git a/rmw/test/test_topic_info_array.cpp b/rmw/test/test_topic_info_array.cpp index ec59f348..54b682b3 100644 --- a/rmw/test/test_topic_info_array.cpp +++ b/rmw/test/test_topic_info_array.cpp @@ -29,10 +29,10 @@ TEST(test_topic_info_array, check_zero) { rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); EXPECT_EQ(rmw_topic_info_array_check_zero(&arr), RMW_RET_OK); rmw_topic_info_array_t arr_count_not_zero = {1, nullptr}; - EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_count_not_zero), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_count_not_zero), RMW_RET_ERROR); rmw_topic_info_t topic_info; rmw_topic_info_array_t arr_info_array_not_null = {0, &topic_info}; - EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_info_array_not_null), RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_info_array_not_null), RMW_RET_ERROR); EXPECT_EQ(rmw_topic_info_array_check_zero(nullptr), RMW_RET_INVALID_ARGUMENT); } From a1c35de28700d55de4ed12df20ef0e1f73e3669c Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 18 Dec 2019 10:23:23 -0800 Subject: [PATCH 24/31] address PR comments Signed-off-by: Miaofei --- rmw/include/rmw/get_topic_info.h | 10 ++++++---- rmw/include/rmw/topic_info.h | 5 +++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/rmw/include/rmw/get_topic_info.h b/rmw/include/rmw/get_topic_info.h index 6d6e7149..599869d7 100644 --- a/rmw/include/rmw/get_topic_info.h +++ b/rmw/include/rmw/get_topic_info.h @@ -36,8 +36,9 @@ extern "C" * 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_info_array_t. The `count` field inside the struct - * must be set to 0 and the `info_array` field inside the struct must be set to null. + * to a valid struct of type rmw_topic_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. * @see rmw_get_zero_initialized_topic_info_array * * The `allocator` will be used to allocate memory to the `info_array` member @@ -85,8 +86,9 @@ rmw_get_publishers_info_by_topic( * 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_info_array_t. The `count` field inside the struct - * must be set to 0 and the `info_array` field inside the struct must be set to null. + * to a valid struct of type rmw_topic_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. * @see rmw_get_zero_initialized_topic_info_array * * The `allocator` will be used to allocate memory to the `info_array` member diff --git a/rmw/include/rmw/topic_info.h b/rmw/include/rmw/topic_info.h index 6aec7fa4..1553c977 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -49,8 +49,9 @@ rmw_get_zero_initialized_topic_info(void); /// Finalize a rmw_topic_info_t object. /** * The rmw_topic_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. + * setting values. + * This function reclaims any allocated resources within the object and zeroes out all other + * members. * * \param[inout] topic_info object to be finalized * \param[in] allocator the allocator used to allocate memory to the object From 747c4c92726b322db0ac256b4d33dc1504546754 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 2 Jan 2020 10:37:58 -0800 Subject: [PATCH 25/31] add rmw_qos_profile_unknown Signed-off-by: Miaofei --- rmw/include/rmw/qos_profiles.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/rmw/include/rmw/qos_profiles.h b/rmw/include/rmw/qos_profiles.h index 90038307..930801e2 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, + 0, + 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 From 06bb0939fedde98699d4115453fb3d92352ec48d Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 2 Jan 2020 12:39:08 -0800 Subject: [PATCH 26/31] use constant instead of literal for history depth Signed-off-by: Miaofei --- rmw/include/rmw/qos_profiles.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw/include/rmw/qos_profiles.h b/rmw/include/rmw/qos_profiles.h index 930801e2..0e39c6f2 100644 --- a/rmw/include/rmw/qos_profiles.h +++ b/rmw/include/rmw/qos_profiles.h @@ -103,7 +103,7 @@ static const rmw_qos_profile_t rmw_qos_profile_system_default = static const rmw_qos_profile_t rmw_qos_profile_unknown = { RMW_QOS_POLICY_HISTORY_UNKNOWN, - 0, + RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT, RMW_QOS_POLICY_RELIABILITY_UNKNOWN, RMW_QOS_POLICY_DURABILITY_UNKNOWN, RMW_QOS_DEADLINE_DEFAULT, From c82859bc62ab73e657adeca28dcf3300268eae2e Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 16:06:27 -0800 Subject: [PATCH 27/31] address more PR comments Signed-off-by: Miaofei --- rmw/include/rmw/get_topic_info.h | 38 ++++----- rmw/include/rmw/rmw.h | 2 +- rmw/include/rmw/topic_info.h | 69 +++++----------- rmw/include/rmw/types.h | 8 ++ rmw/src/topic_info.c | 136 +++++++++++++++++-------------- rmw/src/topic_info_array.c | 10 +-- rmw/test/test_topic_info.cpp | 11 ++- 7 files changed, 133 insertions(+), 141 deletions(-) diff --git a/rmw/include/rmw/get_topic_info.h b/rmw/include/rmw/get_topic_info.h index 599869d7..b3fdd5cd 100644 --- a/rmw/include/rmw/get_topic_info.h +++ b/rmw/include/rmw/get_topic_info.h @@ -23,7 +23,7 @@ extern "C" #include "rmw/topic_info_array.h" #include "rmw/visibility_control.h" -/// Retrieves the information for all publishers to a given topic. +/// 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. @@ -32,25 +32,24 @@ extern "C" * * 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 - * Non existent topic names are allowed. + * 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_info_array_t. - * The `count` field inside the struct must be set to 0 and the `info_array` field inside - * the struct must be set to null. - * @see rmw_get_zero_initialized_topic_info_array + * The rmw_topic_info_array_t struct must be zero initialized. + * \see rmw_get_zero_initialized_topic_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 + * Moreover, every `const char *` member inside of * rmw_topic_info_t will be assigned a copied value on allocated memory. - * @see rmw_topic_info_set_topic_type - * @see rmw_topic_info_set_node_name - * @see rmw_topic_info_set_node_namespace + * \see rmw_topic_info_set_topic_type + * \see rmw_topic_info_set_node_name + * \see rmw_topic_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_info_array_fini + * \see rmw_topic_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. @@ -73,7 +72,7 @@ rmw_get_publishers_info_by_topic( bool no_mangle, rmw_topic_info_array_t * publishers_info); -/// Retrieves the information for all subscriptions to a given topic. +/// 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. @@ -82,25 +81,24 @@ rmw_get_publishers_info_by_topic( * * 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 - * Non existent topic names are allowed. + * 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_info_array_t. - * The `count` field inside the struct must be set to 0 and the `info_array` field inside - * the struct must be set to null. - * @see rmw_get_zero_initialized_topic_info_array + * The rmw_topic_info_array_t struct must be zero initialized. + * \see rmw_get_zero_initialized_topic_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 + * Moreover, every `const char *` member inside of * rmw_topic_info_t will be assigned a copied value on allocated memory. - * @see rmw_topic_info_set_topic_type - * @see rmw_topic_info_set_node_name - * @see rmw_topic_info_set_node_namespace + * \see rmw_topic_info_set_topic_type + * \see rmw_topic_info_set_node_name + * \see rmw_topic_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_info_array_fini + * \see rmw_topic_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. diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index 059b3203..5e5fb288 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -55,8 +55,8 @@ * - rmw_get_service_names_and_types() * - rmw/names_and_types.h * - rmw/get_topic_names_and_types.h - * - rmw/get_service_names_and_types.h * - rmw/get_topic_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_info.h b/rmw/include/rmw/topic_info.h index 1553c977..72829033 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_info.h @@ -25,7 +25,7 @@ extern "C" #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 +/// of publishers and subscriptions for a topic. typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t { /// Name of the node @@ -34,9 +34,11 @@ typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t const char * node_namespace; /// The associated topic type const char * topic_type; - /// The GID of the node - uint8_t gid[RMW_GID_STORAGE_SIZE]; - /// Qos profile of the node + /// 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_info_t; @@ -126,23 +128,24 @@ rmw_topic_info_set_node_namespace( const char * node_namespace, rcutils_allocator_t * allocator); -/// Set the qos_profile in rmw_topic_info_t. +/// Set the gid in rmw_topic_info_t. /** - * rmw_topic_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. + * Copies the values from gid into the gid member inside topic_info. * * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] qos_profile the qos_profile to set in rmw_topic_info_t - * \returns `RMW_RET_OK` on successfully setting the qos_profile, or + * \param[in] gid the gid value to set in rmw_topic_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_info_set_qos_profile( +rmw_topic_info_set_endpoint_type( rmw_topic_info_t * topic_info, - const rmw_qos_profile_t * qos_profile); + rmw_endpoint_type_t type); /// Set the gid in rmw_topic_info_t. /** @@ -164,53 +167,23 @@ rmw_topic_info_set_gid( const uint8_t gid[], size_t size); -/// Free the memory which was allocated for topic_info.node_name -/** - * - * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] allocator the allocator that was used to allocate memory - * \returns `RMW_RET_OK` on successfully freeing the 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_info_fini_node_name( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator); - -/// Free the memory which was allocated for topic_info.node_namespace -/** - * - * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] allocator the allocator that was used to allocate memory - * \returns `RMW_RET_OK` on successfully freeing the 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_info_fini_node_namespace( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator); - -/// Free the memory which was allocated for topic_info.topic_type +/// Set the qos_profile in rmw_topic_info_t. /** + * rmw_topic_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_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] allocator the allocator that was used to allocate memory - * \returns `RMW_RET_OK` on successfully freeing the memory, or + * \param[in] qos_profile the qos_profile to set in rmw_topic_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_info_fini_topic_type( +rmw_topic_info_set_qos_profile( rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator); + const rmw_qos_profile_t * qos_profile); #ifdef __cplusplus } diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index c68e00f7..f552ed1b 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -46,6 +46,14 @@ 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_UNKNOWN +} 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/src/topic_info.c b/rmw/src/topic_info.c index d01974e0..f113c593 100644 --- a/rmw/src/topic_info.c +++ b/rmw/src/topic_info.c @@ -14,6 +14,7 @@ #include "rmw/topic_info.h" +#include "rcutils/strdup.h" #include "rmw/error_handling.h" #include "rmw/types.h" @@ -24,6 +25,52 @@ rmw_get_zero_initialized_topic_info(void) return zero; } +rmw_ret_t +_rmw_topic_info_fini_str( + const char ** topic_info_str, + rcutils_allocator_t * allocator) +{ + allocator->deallocate((char *) *topic_info_str, allocator->state); + *topic_info_str = NULL; + return RMW_RET_OK; +} + +rmw_ret_t +_rmw_topic_info_fini_node_name( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info->node_name) { + RMW_SET_ERROR_MSG("topic_info->node_name is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->node_name, allocator); +} + +rmw_ret_t +_rmw_topic_info_fini_node_namespace( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info->node_namespace) { + RMW_SET_ERROR_MSG("topic_info->node_namespace is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->node_namespace, allocator); +} + +rmw_ret_t +_rmw_topic_info_fini_topic_type( + rmw_topic_info_t * topic_info, + rcutils_allocator_t * allocator) +{ + if (!topic_info->topic_type) { + RMW_SET_ERROR_MSG("topic_info->topic_type is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_info_fini_str(&topic_info->topic_type, allocator); +} + rmw_ret_t rmw_topic_info_fini( rmw_topic_info_t * topic_info, @@ -37,10 +84,23 @@ rmw_topic_info_fini( RMW_SET_ERROR_MSG("allocator is null"); return RMW_RET_INVALID_ARGUMENT; } - allocator->deallocate((char *) topic_info->node_name, allocator->state); - allocator->deallocate((char *) topic_info->node_namespace, allocator->state); - allocator->deallocate((char *) topic_info->topic_type, allocator->state); + + rmw_ret_t ret; + ret = _rmw_topic_info_fini_node_name(topic_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = _rmw_topic_info_fini_node_namespace(topic_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = _rmw_topic_info_fini_topic_type(topic_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + *topic_info = rmw_get_zero_initialized_topic_info(); + return RMW_RET_OK; } @@ -65,10 +125,8 @@ _rmw_topic_info_copy_str( return RMW_RET_INVALID_ARGUMENT; } - size_t size = strlen(str) + 1; - char * temp_str = allocator->allocate(size, allocator->state); - memcpy(temp_str, str, size); - *topic_info_str = temp_str; + *topic_info_str = rcutils_strdup(str, *allocator); + return RMW_RET_OK; } @@ -112,21 +170,17 @@ rmw_topic_info_set_node_namespace( } rmw_ret_t -rmw_topic_info_set_qos_profile( +rmw_topic_info_set_endpoint_type( rmw_topic_info_t * topic_info, - const rmw_qos_profile_t * qos_profile) + rmw_endpoint_type_t type) { if (!topic_info) { RMW_SET_ERROR_MSG("topic_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_info->endpoint_type = type; - topic_info->qos_profile = *qos_profile; return RMW_RET_OK; } @@ -144,64 +198,26 @@ rmw_topic_info_set_gid( RMW_SET_ERROR_MSG("size is more than RMW_GID_STORAGE_SIZE"); return RMW_RET_INVALID_ARGUMENT; } - memset(topic_info->gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(topic_info->gid, gid, size); + memset(topic_info->endpoint_gid, 0, RMW_GID_STORAGE_SIZE); + memcpy(topic_info->endpoint_gid, gid, size); return RMW_RET_OK; } rmw_ret_t -_rmw_topic_info_fini_str( - const char ** topic_info_str, - rcutils_allocator_t * allocator) -{ - if (!topic_info_str) { - RMW_SET_ERROR_MSG("topic_info_str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - allocator->deallocate((char *) *topic_info_str, allocator->state); - *topic_info_str = NULL; - - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_fini_node_name( +rmw_topic_info_set_qos_profile( rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) + const rmw_qos_profile_t * qos_profile) { if (!topic_info) { RMW_SET_ERROR_MSG("topic_info is null"); return RMW_RET_INVALID_ARGUMENT; } - return _rmw_topic_info_fini_str(&topic_info->node_name, allocator); -} -rmw_ret_t -rmw_topic_info_fini_node_namespace( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); + if (!qos_profile) { + RMW_SET_ERROR_MSG("qos_profile is null"); return RMW_RET_INVALID_ARGUMENT; } - return _rmw_topic_info_fini_str(&topic_info->node_namespace, allocator); -} -rmw_ret_t -rmw_topic_info_fini_topic_type( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_fini_str(&topic_info->topic_type, allocator); + topic_info->qos_profile = *qos_profile; + return RMW_RET_OK; } diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c index b5104940..7ef944a9 100644 --- a/rmw/src/topic_info_array.c +++ b/rmw/src/topic_info_array.c @@ -78,15 +78,7 @@ rmw_topic_info_array_fini( rmw_ret_t ret; // free all const char * inside the topic_info_t for (size_t i = 0u; i < topic_info_array->count; i++) { - ret = rmw_topic_info_fini_node_name(&topic_info_array->info_array[i], allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = rmw_topic_info_fini_node_namespace(&topic_info_array->info_array[i], allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = rmw_topic_info_fini_topic_type(&topic_info_array->info_array[i], allocator); + ret = rmw_topic_info_fini(&topic_info_array->info_array[i], allocator); if (ret != RMW_RET_OK) { return ret; } diff --git a/rmw/test/test_topic_info.cpp b/rmw/test/test_topic_info.cpp index 1d04e107..866f9771 100644 --- a/rmw/test/test_topic_info.cpp +++ b/rmw/test/test_topic_info.cpp @@ -102,7 +102,7 @@ TEST(test_topic_info, set_gid) { ret = rmw_topic_info_set_gid(&topic_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_info.gid[i], gid[i]) << "Gid value is not as expected"; + EXPECT_EQ(topic_info.endpoint_gid[i], gid[i]) << "Gid value is not as expected"; } } @@ -151,8 +151,9 @@ TEST(test_topic_info, zero_init) { EXPECT_FALSE(topic_info.node_name); EXPECT_FALSE(topic_info.node_namespace); EXPECT_FALSE(topic_info.topic_type); + EXPECT_EQ(topic_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_info.gid[i], 0) << "Gid value should be 0"; + EXPECT_EQ(topic_info.endpoint_gid[i], 0) << "Gid value should be 0"; } EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; @@ -190,6 +191,8 @@ TEST(test_topic_info, fini) { for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { gid[i] = i * 12; } + ret = rmw_topic_info_set_endpoint_type(&topic_info, RMW_ENDPOINT_PUBLISHER); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid rmw_endpoint_type_t arguments"; ret = rmw_topic_info_set_gid(&topic_info, gid, RMW_GID_STORAGE_SIZE); EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid gid arguments"; ret = rmw_topic_info_set_node_namespace(&topic_info, "namespace", &allocator); @@ -209,8 +212,10 @@ TEST(test_topic_info, fini) { EXPECT_FALSE(topic_info.node_name); EXPECT_FALSE(topic_info.node_namespace); EXPECT_FALSE(topic_info.topic_type); + EXPECT_EQ(topic_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_info.gid[i], 0) << "Gid value should be 0"; + EXPECT_EQ(topic_info.endpoint_gid[i], 0) << "Gid value should be 0"; } EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; From 702aef298ef38d233803a22e6d6d98849ea4497b Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 23:44:21 -0800 Subject: [PATCH 28/31] rename *topic_info* to *topic_endpoint_info* Signed-off-by: Miaofei --- rmw/CMakeLists.txt | 4 +- ...topic_info.h => get_topic_endpoint_info.h} | 48 ++-- rmw/include/rmw/rmw.h | 2 +- .../{topic_info.h => topic_endpoint_info.h} | 98 ++++---- ...fo_array.h => topic_endpoint_info_array.h} | 64 ++--- rmw/src/topic_endpoint_info.c | 223 +++++++++++++++++ rmw/src/topic_endpoint_info_array.c | 91 +++++++ rmw/src/topic_info.c | 223 ----------------- rmw/src/topic_info_array.c | 91 ------- rmw/test/CMakeLists.txt | 16 +- rmw/test/test_topic_endpoint_info.cpp | 235 ++++++++++++++++++ rmw/test/test_topic_endpoint_info_array.cpp | 63 +++++ rmw/test/test_topic_info.cpp | 235 ------------------ rmw/test/test_topic_info_array.cpp | 63 ----- 14 files changed, 728 insertions(+), 728 deletions(-) rename rmw/include/rmw/{get_topic_info.h => get_topic_endpoint_info.h} (76%) rename rmw/include/rmw/{topic_info.h => topic_endpoint_info.h} (61%) rename rmw/include/rmw/{topic_info_array.h => topic_endpoint_info_array.h} (50%) create mode 100644 rmw/src/topic_endpoint_info.c create mode 100644 rmw/src/topic_endpoint_info_array.c delete mode 100644 rmw/src/topic_info.c delete mode 100644 rmw/src/topic_info_array.c create mode 100644 rmw/test/test_topic_endpoint_info.cpp create mode 100644 rmw/test/test_topic_endpoint_info_array.cpp delete mode 100644 rmw/test/test_topic_info.cpp delete mode 100644 rmw/test/test_topic_info_array.cpp diff --git a/rmw/CMakeLists.txt b/rmw/CMakeLists.txt index 7c0221a0..035074cf 100644 --- a/rmw/CMakeLists.txt +++ b/rmw/CMakeLists.txt @@ -36,8 +36,8 @@ set(rmw_sources "src/publisher_options.c" "src/sanity_checks.c" "src/subscription_options.c" - "src/topic_info_array.c" - "src/topic_info.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_info.h b/rmw/include/rmw/get_topic_endpoint_info.h similarity index 76% rename from rmw/include/rmw/get_topic_info.h rename to rmw/include/rmw/get_topic_endpoint_info.h index b3fdd5cd..efb5b42f 100644 --- a/rmw/include/rmw/get_topic_info.h +++ b/rmw/include/rmw/get_topic_endpoint_info.h @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW__GET_TOPIC_INFO_H_ -#define RMW__GET_TOPIC_INFO_H_ +#ifndef RMW__GET_TOPIC_ENDPOINT_INFO_H_ +#define RMW__GET_TOPIC_ENDPOINT_INFO_H_ #ifdef __cplusplus extern "C" { #endif -#include "rmw/topic_info_array.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/visibility_control.h" /// Retrieve the information for all publishers to a given topic. @@ -36,26 +36,26 @@ extern "C" * 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_info_array_t. - * The rmw_topic_info_array_t struct must be zero initialized. - * \see rmw_get_zero_initialized_topic_info_array + * 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_info_t will be assigned a copied value on allocated memory. - * \see rmw_topic_info_set_topic_type - * \see rmw_topic_info_set_node_name - * \see rmw_topic_info_set_node_namespace + * 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_info_array_fini + * \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_info_t. + * \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 @@ -70,7 +70,7 @@ rmw_get_publishers_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * publishers_info); + rmw_topic_endpoint_info_array_t * publishers_info); /// Retrieve the information for all subscriptions to a given topic. /** @@ -85,26 +85,26 @@ rmw_get_publishers_info_by_topic( * 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_info_array_t. - * The rmw_topic_info_array_t struct must be zero initialized. - * \see rmw_get_zero_initialized_topic_info_array + * 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_info_t will be assigned a copied value on allocated memory. - * \see rmw_topic_info_set_topic_type - * \see rmw_topic_info_set_node_name - * \see rmw_topic_info_set_node_namespace + * 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_info_array_fini + * \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_info_t.. + * \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 @@ -119,10 +119,10 @@ rmw_get_subscriptions_info_by_topic( rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - rmw_topic_info_array_t * subscriptions_info); + rmw_topic_endpoint_info_array_t * subscriptions_info); #ifdef __cplusplus } #endif -#endif // RMW__GET_TOPIC_INFO_H_ +#endif // RMW__GET_TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index 5e5fb288..2fb995f1 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -55,7 +55,7 @@ * - rmw_get_service_names_and_types() * - rmw/names_and_types.h * - rmw/get_topic_names_and_types.h - * - rmw/get_topic_info.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_info.h b/rmw/include/rmw/topic_endpoint_info.h similarity index 61% rename from rmw/include/rmw/topic_info.h rename to rmw/include/rmw/topic_endpoint_info.h index 72829033..b6f5ffcd 100644 --- a/rmw/include/rmw/topic_info.h +++ b/rmw/include/rmw/topic_endpoint_info.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW__TOPIC_INFO_H_ -#define RMW__TOPIC_INFO_H_ +#ifndef RMW__TOPIC_ENDPOINT_INFO_H_ +#define RMW__TOPIC_ENDPOINT_INFO_H_ #ifdef __cplusplus extern "C" @@ -26,7 +26,7 @@ extern "C" /// 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_info_t +typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_t { /// Name of the node const char * node_name; @@ -40,22 +40,22 @@ typedef struct RMW_PUBLIC_TYPE rmw_topic_info_t uint8_t endpoint_gid[RMW_GID_STORAGE_SIZE]; /// QoS profile of the endpoint rmw_qos_profile_t qos_profile; -} rmw_topic_info_t; +} rmw_topic_endpoint_info_t; -/// Return a rmw_topic_info_t struct with members initialized to `NULL`. +/// Return a rmw_topic_endpoint_info_t struct with members initialized to `NULL`. RMW_PUBLIC RMW_WARN_UNUSED -rmw_topic_info_t -rmw_get_zero_initialized_topic_info(void); +rmw_topic_endpoint_info_t +rmw_get_zero_initialized_topic_endpoint_info(void); -/// Finalize a rmw_topic_info_t object. +/// Finalize a rmw_topic_endpoint_info_t object. /** - * The rmw_topic_info_t struct has members which require memory to be allocated to them before + * 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_info object to be finalized + * \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 @@ -64,17 +64,17 @@ rmw_get_zero_initialized_topic_info(void); RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_fini( - rmw_topic_info_t * topic_info, +rmw_topic_endpoint_info_fini( + rmw_topic_endpoint_info_t * topic_endpoint_info, rcutils_allocator_t * allocator); -/// Set the topic_type in rmw_topic_info_t. +/// Set the topic_type in rmw_topic_endpoint_info_t. /** - * rmw_topic_info_t has a member topic_type of type const char *; + * 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_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] topic_type the topic_type value to set in rmw_topic_info_t + * \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 @@ -83,18 +83,18 @@ rmw_topic_info_fini( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_topic_type( - rmw_topic_info_t * topic_info, +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_info_t. +/// Set the node_name in rmw_topic_endpoint_info_t. /** - * rmw_topic_info_t has a member node_name of type const char *; + * 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_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] node_name the node_name value to set in rmw_topic_info_t + * \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 @@ -103,18 +103,18 @@ rmw_topic_info_set_topic_type( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_node_name( - rmw_topic_info_t * topic_info, +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_info_t. +/// Set the node_namespace in rmw_topic_endpoint_info_t. /** - * rmw_topic_info_t has a member node_namespace of type const char *; + * 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_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] node_namespace the node_namespace value to set in rmw_topic_info_t + * \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 @@ -123,17 +123,17 @@ rmw_topic_info_set_node_name( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_node_namespace( - rmw_topic_info_t * topic_info, +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_info_t. +/// Set the gid in rmw_topic_endpoint_info_t. /** - * Copies the values from gid into the gid member inside topic_info. + * Copies the values from gid into the gid member inside topic_endpoint_info. * - * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] gid the gid value to set in rmw_topic_info_t + * \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 @@ -143,16 +143,16 @@ rmw_topic_info_set_node_namespace( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_endpoint_type( - rmw_topic_info_t * topic_info, +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_info_t. +/// Set the gid in rmw_topic_endpoint_info_t. /** - * Copies the values from gid into the gid member inside topic_info. + * Copies the values from gid into the gid member inside topic_endpoint_info. * - * \param[inout] topic_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] gid the gid value to set in rmw_topic_info_t + * \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 @@ -162,18 +162,18 @@ rmw_topic_info_set_endpoint_type( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_gid( - rmw_topic_info_t * topic_info, +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_info_t. +/// Set the qos_profile in rmw_topic_endpoint_info_t. /** - * rmw_topic_info_t has a member qos_profile of type const rmw_qos_profile_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_info pointer to an initialized instance of rmw_topic_info_t - * \param[in] qos_profile the qos_profile to set in rmw_topic_info_t + * \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. @@ -181,12 +181,12 @@ rmw_topic_info_set_gid( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_set_qos_profile( - rmw_topic_info_t * topic_info, +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_INFO_H_ +#endif // RMW__TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw/include/rmw/topic_info_array.h b/rmw/include/rmw/topic_endpoint_info_array.h similarity index 50% rename from rmw/include/rmw/topic_info_array.h rename to rmw/include/rmw/topic_endpoint_info_array.h index b5f1530a..2e5ccfab 100644 --- a/rmw/include/rmw/topic_info_array.h +++ b/rmw/include/rmw/topic_endpoint_info_array.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW__TOPIC_INFO_ARRAY_H_ -#define RMW__TOPIC_INFO_ARRAY_H_ +#ifndef RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ +#define RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ #ifdef __cplusplus extern "C" @@ -21,73 +21,73 @@ extern "C" #endif #include "rcutils/allocator.h" -#include "rmw/topic_info.h" +#include "rmw/topic_endpoint_info.h" #include "rmw/visibility_control.h" -/// Array of rmw_topic_info_t -typedef struct RMW_PUBLIC_TYPE rmw_topic_info_array_t +/// 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_info_t - rmw_topic_info_t * info_array; -} rmw_topic_info_array_t; + /// 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_info_array_t struct with members initialized to `NULL`. +/// Return a rmw_topic_endpoint_info_array_t struct with members initialized to `NULL`. RMW_PUBLIC RMW_WARN_UNUSED -rmw_topic_info_array_t -rmw_get_zero_initialized_topic_info_array(void); +rmw_topic_endpoint_info_array_t +rmw_get_zero_initialized_topic_endpoint_info_array(void); -/// Check that a rmw_topic_info_array_t struct is zero initialized. +/// Check that a rmw_topic_endpoint_info_array_t struct is zero initialized. /** - * This function checks if the provided rmw_topic_info_array_t is zero initialized or not. + * This function checks if the provided rmw_topic_endpoint_info_array_t is zero initialized or not. * - * \param[in] topic_info_array the data structure to be checked - * \returns `RMW_RET_OK` if topic_info_array is zero initialized + * \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_info_array is not zero initialized + * \returns `RMW_RET_ERROR` if topic_endpoint_info_array is not zero initialized */ RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array); +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_info_array_t with the given size +/// Initialize the info_array member inside rmw_topic_endpoint_info_array_t with the given size /** - * The rmw_topic_info_array_t has a member variable info_array which is an array of - * type rmw_topic_info_t. + * 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_info_array must be zero initialized before being passed into this function. + * topic_endpoint_info_array must be zero initialized before being passed into this function. * - * \param[inout] topic_info_array the data structure to initialise + * \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_info_array is not zero initialized, 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_info_array_init_with_size( - rmw_topic_info_array_t * topic_info_array, +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_info_array_t object. +/// Finalize a rmw_topic_endpoint_info_array_t object. /** - * The info_array member variable inside of rmw_topic_info_array represents an array of - * rmw_topic_info_t. + * 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_info_array object to be finalized + * \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 @@ -96,12 +96,12 @@ rmw_topic_info_array_init_with_size( RMW_PUBLIC RMW_WARN_UNUSED rmw_ret_t -rmw_topic_info_array_fini( - rmw_topic_info_array_t * topic_info_array, +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_INFO_ARRAY_H_ +#endif // RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ diff --git a/rmw/src/topic_endpoint_info.c b/rmw/src/topic_endpoint_info.c new file mode 100644 index 00000000..c4c924e4 --- /dev/null +++ b/rmw/src/topic_endpoint_info.c @@ -0,0 +1,223 @@ +// 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) +{ + rmw_topic_endpoint_info_t zero = {0}; + 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..af648c94 --- /dev/null +++ b/rmw/src/topic_endpoint_info_array.c @@ -0,0 +1,91 @@ +// 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) +{ + rmw_topic_endpoint_info_array_t zero = {0}; + 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/src/topic_info.c b/rmw/src/topic_info.c deleted file mode 100644 index f113c593..00000000 --- a/rmw/src/topic_info.c +++ /dev/null @@ -1,223 +0,0 @@ -// 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_info.h" - -#include "rcutils/strdup.h" -#include "rmw/error_handling.h" -#include "rmw/types.h" - -rmw_topic_info_t -rmw_get_zero_initialized_topic_info(void) -{ - rmw_topic_info_t zero = {0}; - return zero; -} - -rmw_ret_t -_rmw_topic_info_fini_str( - const char ** topic_info_str, - rcutils_allocator_t * allocator) -{ - allocator->deallocate((char *) *topic_info_str, allocator->state); - *topic_info_str = NULL; - return RMW_RET_OK; -} - -rmw_ret_t -_rmw_topic_info_fini_node_name( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info->node_name) { - RMW_SET_ERROR_MSG("topic_info->node_name is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_fini_str(&topic_info->node_name, allocator); -} - -rmw_ret_t -_rmw_topic_info_fini_node_namespace( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info->node_namespace) { - RMW_SET_ERROR_MSG("topic_info->node_namespace is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_fini_str(&topic_info->node_namespace, allocator); -} - -rmw_ret_t -_rmw_topic_info_fini_topic_type( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info->topic_type) { - RMW_SET_ERROR_MSG("topic_info->topic_type is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_fini_str(&topic_info->topic_type, allocator); -} - -rmw_ret_t -rmw_topic_info_fini( - rmw_topic_info_t * topic_info, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_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_info_fini_node_name(topic_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = _rmw_topic_info_fini_node_namespace(topic_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = _rmw_topic_info_fini_topic_type(topic_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - - *topic_info = rmw_get_zero_initialized_topic_info(); - - return RMW_RET_OK; -} - -rmw_ret_t -_rmw_topic_info_copy_str( - const char ** topic_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_info_str) { - RMW_SET_ERROR_MSG("topic_info_str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - *topic_info_str = rcutils_strdup(str, *allocator); - - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_set_topic_type( - rmw_topic_info_t * topic_info, - const char * topic_type, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(&topic_info->topic_type, topic_type, allocator); -} - -rmw_ret_t -rmw_topic_info_set_node_name( - rmw_topic_info_t * topic_info, - const char * node_name, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(&topic_info->node_name, node_name, allocator); -} - -rmw_ret_t -rmw_topic_info_set_node_namespace( - rmw_topic_info_t * topic_info, - const char * node_namespace, - rcutils_allocator_t * allocator) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_info_copy_str(&topic_info->node_namespace, node_namespace, allocator); -} - -rmw_ret_t -rmw_topic_info_set_endpoint_type( - rmw_topic_info_t * topic_info, - rmw_endpoint_type_t type) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - topic_info->endpoint_type = type; - - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_set_gid( - rmw_topic_info_t * topic_info, - const uint8_t gid[], - size_t size) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_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_info->endpoint_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(topic_info->endpoint_gid, gid, size); - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_set_qos_profile( - rmw_topic_info_t * topic_info, - const rmw_qos_profile_t * qos_profile) -{ - if (!topic_info) { - RMW_SET_ERROR_MSG("topic_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_info->qos_profile = *qos_profile; - return RMW_RET_OK; -} diff --git a/rmw/src/topic_info_array.c b/rmw/src/topic_info_array.c deleted file mode 100644 index 7ef944a9..00000000 --- a/rmw/src/topic_info_array.c +++ /dev/null @@ -1,91 +0,0 @@ -// 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_info_array.h" -#include "rmw/error_handling.h" -#include "rmw/types.h" - -rmw_topic_info_array_t -rmw_get_zero_initialized_topic_info_array(void) -{ - rmw_topic_info_array_t zero = {0}; - return zero; -} - -rmw_ret_t -rmw_topic_info_array_check_zero(rmw_topic_info_array_t * topic_info_array) -{ - if (!topic_info_array) { - RMW_SET_ERROR_MSG("topic_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - if (topic_info_array->count != 0 || topic_info_array->info_array != NULL) { - RMW_SET_ERROR_MSG("topic_info_array is not zeroed"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_info_array_init_with_size( - rmw_topic_info_array_t * topic_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_info_array) { - RMW_SET_ERROR_MSG("topic_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - topic_info_array->info_array = allocator->allocate(sizeof(*topic_info_array->info_array) * size, - allocator->state); - if (!topic_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_info_array_fini( - rmw_topic_info_array_t * topic_info_array, - rcutils_allocator_t * allocator) -{ - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!topic_info_array) { - RMW_SET_ERROR_MSG("topic_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - rmw_ret_t ret; - // free all const char * inside the topic_info_t - for (size_t i = 0u; i < topic_info_array->count; i++) { - ret = rmw_topic_info_fini(&topic_info_array->info_array[i], allocator); - if (ret != RMW_RET_OK) { - return ret; - } - } - - allocator->deallocate(topic_info_array->info_array, allocator->state); - topic_info_array->info_array = NULL; - topic_info_array->count = 0; - return RMW_RET_OK; -} diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index bd393d4c..cba7748e 100644 --- a/rmw/test/CMakeLists.txt +++ b/rmw/test/CMakeLists.txt @@ -43,22 +43,22 @@ if(TARGET test_validate_namespace) endif() endif() -ament_add_gmock(test_topic_info_array - test_topic_info_array.cpp +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_info_array) - target_link_libraries(test_topic_info_array ${PROJECT_NAME} +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_info - test_topic_info.cpp +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_info) - target_link_libraries(test_topic_info ${PROJECT_NAME} +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..834e4dc4 --- /dev/null +++ b/rmw/test/test_topic_endpoint_info.cpp @@ -0,0 +1,235 @@ +// 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..b971140b --- /dev/null +++ b/rmw/test/test_topic_endpoint_info_array.cpp @@ -0,0 +1,63 @@ +// 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); +} diff --git a/rmw/test/test_topic_info.cpp b/rmw/test/test_topic_info.cpp deleted file mode 100644 index 866f9771..00000000 --- a/rmw/test/test_topic_info.cpp +++ /dev/null @@ -1,235 +0,0 @@ -// 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_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_info, set_topic_type) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * val = get_mallocd_string("test_topic_type"); - rmw_ret_t ret = rmw_topic_info_set_topic_type(&topic_info, val, nullptr); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; - ret = rmw_topic_info_set_topic_type(&topic_info, nullptr, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_type"; - ret = rmw_topic_info_set_topic_type(nullptr, val, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - allocator.deallocate(const_cast(topic_info.topic_type), allocator.state); - }); - ret = rmw_topic_info_set_topic_type(&topic_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_info.topic_type, "test_topic_type") << "Topic Type value is not as expected"; -} - -TEST(test_topic_info, set_node_name) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * val = get_mallocd_string("test_node_name"); - rmw_ret_t ret = rmw_topic_info_set_node_name(&topic_info, val, nullptr); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; - ret = rmw_topic_info_set_node_name(&topic_info, nullptr, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_name"; - ret = rmw_topic_info_set_node_name(nullptr, val, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - allocator.deallocate(const_cast(topic_info.node_name), allocator.state); - }); - ret = rmw_topic_info_set_node_name(&topic_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_info.node_name, "test_node_name") << "Node name value is not as expected"; -} - -TEST(test_topic_info, set_node_namespace) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * val = get_mallocd_string("test_node_namespace"); - rmw_ret_t ret = rmw_topic_info_set_node_namespace(&topic_info, val, nullptr); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; - ret = rmw_topic_info_set_node_namespace(&topic_info, nullptr, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_namespace"; - ret = rmw_topic_info_set_node_namespace(nullptr, val, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - allocator.deallocate(const_cast(topic_info.node_namespace), allocator.state); - }); - ret = rmw_topic_info_set_node_namespace(&topic_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_info.node_namespace, "test_node_namespace") << "Node namespace value is" - "not as expected"; -} - -TEST(test_topic_info, set_gid) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_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_info_set_gid(nullptr, gid, RMW_GID_STORAGE_SIZE); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - ret = rmw_topic_info_set_gid(&topic_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_info_set_gid(&topic_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_info.endpoint_gid[i], gid[i]) << "Gid value is not as expected"; - } -} - -TEST(test_topic_info, set_qos_profile) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_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_info_set_qos_profile(nullptr, &qos_profile); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - ret = rmw_topic_info_set_qos_profile(&topic_info, nullptr); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null qos_profile"; - ret = rmw_topic_info_set_qos_profile(&topic_info, &qos_profile); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; - - EXPECT_EQ(topic_info.qos_profile.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST) << "Unequal history"; - EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Unequal depth"; - EXPECT_EQ(topic_info.qos_profile.reliability, - RMW_QOS_POLICY_RELIABILITY_RELIABLE) << "Unequal reliability"; - EXPECT_EQ(topic_info.qos_profile.durability, - RMW_QOS_POLICY_DURABILITY_VOLATILE) << "Unequal durability"; - EXPECT_EQ(topic_info.qos_profile.deadline.sec, 1u) << "Unequal deadline sec"; - EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Unequal deadline nsec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 2u) << "Unequal lifespan sec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Unequal lifespan nsec"; - EXPECT_EQ(topic_info.qos_profile.liveliness, - RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) << "Unequal liveliness"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, - 3u) << "Unequal liveliness lease duration sec"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, - 0u) << "Unequal liveliness lease duration nsec"; - EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, - false) << "Unequal avoid namespace conventions"; -} - -TEST(test_topic_info, zero_init) { - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_info(); - EXPECT_FALSE(topic_info.node_name); - EXPECT_FALSE(topic_info.node_namespace); - EXPECT_FALSE(topic_info.topic_type); - EXPECT_EQ(topic_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_info.endpoint_gid[i], 0) << "Gid value should be 0"; - } - EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; - EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; - EXPECT_EQ(topic_info.qos_profile.reliability, 0) << "Non-zero reliability"; - EXPECT_EQ(topic_info.qos_profile.durability, 0) << "Non-zero durability"; - EXPECT_EQ(topic_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; - EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; - EXPECT_EQ(topic_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, - 0u) << "Non-zero liveliness lease duration sec"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, - 0u) << "Non-zero liveliness lease duration nsec"; - EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, - false) << "Non-zero avoid namespace conventions"; -} - -TEST(test_topic_info, fini) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rmw_topic_info_t topic_info = rmw_get_zero_initialized_topic_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_info_set_qos_profile(&topic_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_info_set_endpoint_type(&topic_info, RMW_ENDPOINT_PUBLISHER); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid rmw_endpoint_type_t arguments"; - ret = rmw_topic_info_set_gid(&topic_info, gid, RMW_GID_STORAGE_SIZE); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid gid arguments"; - ret = rmw_topic_info_set_node_namespace(&topic_info, "namespace", &allocator); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_namespace arguments"; - ret = rmw_topic_info_set_node_name(&topic_info, "name", &allocator); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_name arguments"; - ret = rmw_topic_info_set_topic_type(&topic_info, "type", &allocator); - EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid topic_type arguments"; - ret = rmw_topic_info_fini(&topic_info, nullptr); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; - ret = rmw_topic_info_fini(nullptr, &allocator); - EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_info"; - - ret = rmw_topic_info_fini(&topic_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_info.node_name); - EXPECT_FALSE(topic_info.node_namespace); - EXPECT_FALSE(topic_info.topic_type); - EXPECT_EQ(topic_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_info.endpoint_gid[i], 0) << "Gid value should be 0"; - } - EXPECT_EQ(topic_info.qos_profile.history, 0) << "Non-zero history"; - EXPECT_EQ(topic_info.qos_profile.depth, 0u) << "Non-zero depth"; - EXPECT_EQ(topic_info.qos_profile.reliability, 0) << "Non-zero reliability"; - EXPECT_EQ(topic_info.qos_profile.durability, 0) << "Non-zero durability"; - EXPECT_EQ(topic_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; - EXPECT_EQ(topic_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; - EXPECT_EQ(topic_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; - EXPECT_EQ(topic_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.sec, - 0u) << "Non-zero liveliness lease duration sec"; - EXPECT_EQ(topic_info.qos_profile.liveliness_lease_duration.nsec, - 0u) << "Non-zero liveliness lease duration nsec"; - EXPECT_EQ(topic_info.qos_profile.avoid_ros_namespace_conventions, - false) << "Non-zero avoid namespace conventions"; -} diff --git a/rmw/test/test_topic_info_array.cpp b/rmw/test/test_topic_info_array.cpp deleted file mode 100644 index 54b682b3..00000000 --- a/rmw/test/test_topic_info_array.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// 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_info_array.h" -#include "rmw/types.h" - -TEST(test_topic_info_array, zero_initialize) { - rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); - EXPECT_EQ(arr.count, 0u); - EXPECT_FALSE(arr.info_array); -} - -TEST(test_topic_info_array, check_zero) { - rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); - EXPECT_EQ(rmw_topic_info_array_check_zero(&arr), RMW_RET_OK); - rmw_topic_info_array_t arr_count_not_zero = {1, nullptr}; - EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_count_not_zero), RMW_RET_ERROR); - rmw_topic_info_t topic_info; - rmw_topic_info_array_t arr_info_array_not_null = {0, &topic_info}; - EXPECT_EQ(rmw_topic_info_array_check_zero(&arr_info_array_not_null), RMW_RET_ERROR); - EXPECT_EQ(rmw_topic_info_array_check_zero(nullptr), RMW_RET_INVALID_ARGUMENT); -} - -TEST(test_topic_info_array, check_init_with_size) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ - rmw_ret_t fini_ret = rmw_topic_info_array_fini(&arr, &allocator); - EXPECT_EQ(fini_ret, RMW_RET_OK); - }); - EXPECT_EQ(rmw_topic_info_array_init_with_size(&arr, 1, nullptr), RMW_RET_INVALID_ARGUMENT); - EXPECT_EQ(rmw_topic_info_array_init_with_size(nullptr, 1, &allocator), RMW_RET_INVALID_ARGUMENT); - EXPECT_FALSE(arr.info_array); - rmw_ret_t ret = rmw_topic_info_array_init_with_size(&arr, 5, &allocator); - EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_TRUE(arr.info_array); -} - -TEST(test_topic_info_array, check_fini) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rmw_topic_info_array_t arr = rmw_get_zero_initialized_topic_info_array(); - rmw_ret_t ret = rmw_topic_info_array_init_with_size(&arr, 5, &allocator); - EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_TRUE(arr.info_array); - ret = rmw_topic_info_array_fini(&arr, &allocator); - EXPECT_EQ(ret, RMW_RET_OK); - EXPECT_FALSE(arr.info_array); -} From 8ddb815859a971b8dd28dd5d7cecf20b79b03dd8 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 9 Jan 2020 23:57:34 -0800 Subject: [PATCH 29/31] fix formatting Signed-off-by: Miaofei --- rmw/include/rmw/topic_endpoint_info_array.h | 3 +- rmw/src/topic_endpoint_info.c | 5 ++- rmw/src/topic_endpoint_info_array.c | 7 +-- rmw/test/test_topic_endpoint_info.cpp | 50 ++++++++++++--------- rmw/test/test_topic_endpoint_info_array.cpp | 6 ++- 5 files changed, 44 insertions(+), 27 deletions(-) diff --git a/rmw/include/rmw/topic_endpoint_info_array.h b/rmw/include/rmw/topic_endpoint_info_array.h index 2e5ccfab..e736b811 100644 --- a/rmw/include/rmw/topic_endpoint_info_array.h +++ b/rmw/include/rmw/topic_endpoint_info_array.h @@ -51,7 +51,8 @@ rmw_get_zero_initialized_topic_endpoint_info_array(void); 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); +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 /** diff --git a/rmw/src/topic_endpoint_info.c b/rmw/src/topic_endpoint_info.c index c4c924e4..4bb9d35e 100644 --- a/rmw/src/topic_endpoint_info.c +++ b/rmw/src/topic_endpoint_info.c @@ -166,7 +166,10 @@ rmw_topic_endpoint_info_set_node_namespace( 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); + return _rmw_topic_endpoint_info_copy_str( + &topic_endpoint_info->node_namespace, + node_namespace, + allocator); } rmw_ret_t diff --git a/rmw/src/topic_endpoint_info_array.c b/rmw/src/topic_endpoint_info_array.c index af648c94..8e4888b4 100644 --- a/rmw/src/topic_endpoint_info_array.c +++ b/rmw/src/topic_endpoint_info_array.c @@ -24,7 +24,8 @@ rmw_get_zero_initialized_topic_endpoint_info_array(void) } rmw_ret_t -rmw_topic_endpoint_info_array_check_zero(rmw_topic_endpoint_info_array_t * topic_endpoint_info_array) +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"); @@ -51,8 +52,8 @@ rmw_topic_endpoint_info_array_init_with_size( 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); + 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; diff --git a/rmw/test/test_topic_endpoint_info.cpp b/rmw/test/test_topic_endpoint_info.cpp index 834e4dc4..c1fe0166 100644 --- a/rmw/test/test_topic_endpoint_info.cpp +++ b/rmw/test/test_topic_endpoint_info.cpp @@ -36,7 +36,8 @@ TEST(test_topic_endpoint_info, set_topic_type) { 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"; + 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); }); @@ -44,7 +45,8 @@ TEST(test_topic_endpoint_info, set_topic_type) { 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"; + 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) { @@ -56,7 +58,8 @@ TEST(test_topic_endpoint_info, set_node_name) { 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"; + 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); }); @@ -64,7 +67,8 @@ TEST(test_topic_endpoint_info, set_node_name) { 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"; + 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) { @@ -76,7 +80,8 @@ TEST(test_topic_endpoint_info, set_node_namespace) { 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"; + 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); }); @@ -84,8 +89,8 @@ TEST(test_topic_endpoint_info, set_node_namespace) { 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"; + EXPECT_STREQ(topic_endpoint_info.node_namespace, "test_node_namespace") << + "Node namespace value is not as expected"; } TEST(test_topic_endpoint_info, set_gid) { @@ -95,10 +100,11 @@ TEST(test_topic_endpoint_info, set_gid) { 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"; + 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"; + 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++) { @@ -120,13 +126,15 @@ TEST(test_topic_endpoint_info, set_qos_profile) { 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"; + 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.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"; @@ -151,7 +159,8 @@ TEST(test_topic_endpoint_info, zero_init) { 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"; + 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"; } @@ -204,7 +213,8 @@ TEST(test_topic_endpoint_info, fini) { 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"; + 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. @@ -226,10 +236,10 @@ TEST(test_topic_endpoint_info, fini) { 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"; + 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 index b971140b..eb4f80d4 100644 --- a/rmw/test/test_topic_endpoint_info_array.cpp +++ b/rmw/test/test_topic_endpoint_info_array.cpp @@ -43,8 +43,10 @@ TEST(test_topic_endpoint_info_array, check_init_with_size) { 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_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); From 6d9675a7d53bb6e8e1bf009b5c351964f3f43411 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 10 Jan 2020 10:11:01 -0800 Subject: [PATCH 30/31] remove unused enum value from rmw_endpoint_type_t Signed-off-by: Miaofei --- rmw/include/rmw/types.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index f552ed1b..cbbe5dd8 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -50,8 +50,7 @@ typedef enum RMW_PUBLIC_TYPE { RMW_ENDPOINT_INVALID = 0, RMW_ENDPOINT_PUBLISHER, - RMW_ENDPOINT_SUBSCRIPTION, - RMW_ENDPOINT_UNKNOWN + RMW_ENDPOINT_SUBSCRIPTION } rmw_endpoint_type_t; /// Options that can be used to configure the creation of a publisher in rmw. From 2915b73fbc44a1d031a13ea22ad1c9d92abc74b6 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Mon, 13 Jan 2020 14:29:00 -0800 Subject: [PATCH 31/31] fix clang compiler warnings Signed-off-by: Miaofei --- rmw/src/topic_endpoint_info.c | 7 +++++++ rmw/src/topic_endpoint_info_array.c | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/rmw/src/topic_endpoint_info.c b/rmw/src/topic_endpoint_info.c index 4bb9d35e..d09dcbde 100644 --- a/rmw/src/topic_endpoint_info.c +++ b/rmw/src/topic_endpoint_info.c @@ -21,7 +21,14 @@ 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; } diff --git a/rmw/src/topic_endpoint_info_array.c b/rmw/src/topic_endpoint_info_array.c index 8e4888b4..aa6f4b2a 100644 --- a/rmw/src/topic_endpoint_info_array.c +++ b/rmw/src/topic_endpoint_info_array.c @@ -19,7 +19,14 @@ 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; }