From 13ab6c3196af67e3e7c4ae77880163a61dc804cf Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 25 Jan 2022 11:05:34 -0300 Subject: [PATCH] Implement copy function for C messages (#650) Signed-off-by: Michel Hidalgo --- .../resource/msg__functions.c.em | 83 +++++++++++++++++++ .../resource/msg__functions.h.em | 34 ++++++++ rosidl_generator_c/test/test_interfaces.c | 65 +++++++++++++++ .../primitives_sequence_functions.h | 45 +++++++++- .../rosidl_runtime_c/string_functions.h | 36 ++++++++ .../rosidl_runtime_c/u16string_functions.h | 36 ++++++++ .../src/primitives_sequence_functions.c | 47 +++++++++++ rosidl_runtime_c/src/string_functions.c | 54 ++++++++++++ rosidl_runtime_c/src/u16string_functions.c | 55 ++++++++++++ .../test_primitives_sequence_functions.cpp | 17 ++++ .../test/test_string_functions.cpp | 38 +++++++++ .../test/test_u16string_functions.cpp | 40 +++++++++ 12 files changed, 547 insertions(+), 3 deletions(-) diff --git a/rosidl_generator_c/resource/msg__functions.c.em b/rosidl_generator_c/resource/msg__functions.c.em index 5c72068cb..4f1406e83 100644 --- a/rosidl_generator_c/resource/msg__functions.c.em +++ b/rosidl_generator_c/resource/msg__functions.c.em @@ -275,6 +275,47 @@ bool return true; } +bool +@(message_typename)__copy( + const @(message_typename) * input, + @(message_typename) * output) +{ + if (!input || !output) { + return false; + } +@[for member in message.structure.members]@ + // @(member.name) +@[ if isinstance(member.type, Array)]@ + for (size_t i = 0; i < @(member.type.size); ++i) { +@[ if isinstance(member.type.value_type, (AbstractGenericString, NamespacedType))]@ + if (!@(basetype_to_c(member.type.value_type))__copy( + &(input->@(member.name)[i]), &(output->@(member.name)[i]))) + { + return false; + } +@[ else]@ + output->@(member.name)[i] = input->@(member.name)[i]; +@[ end if]@ + } +@[ elif isinstance(member.type, AbstractSequence)]@ + if (!@(idl_type_to_c(member.type))__copy( + &(input->@(member.name)), &(output->@(member.name)))) + { + return false; + } +@[ elif isinstance(member.type, (AbstractGenericString, NamespacedType))]@ + if (!@(basetype_to_c(member.type))__copy( + &(input->@(member.name)), &(output->@(member.name)))) + { + return false; + } +@[ else]@ + output->@(member.name) = input->@(member.name); +@[ end if]@ +@[end for]@ + return true; +} + @(message_typename) * @(message_typename)__create() { @@ -403,3 +444,45 @@ bool } return true; } + +bool +@(array_typename)__copy( + const @(array_typename) * input, + @(array_typename) * output) +{ + if (!input || !output) { + return false; + } + if (output->capacity < input->size) { + const size_t allocation_size = + input->size * sizeof(@(message_typename)); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + @(message_typename) * data = + (@(message_typename) *)allocator.reallocate( + output->data, allocation_size, allocator.state); + if (!data) { + return false; + } + for (size_t i = output->capacity; i < input->size; ++i) { + if (!@(message_typename)__init(&data[i])) { + /* free currently allocated and return false */ + for (; i-- > output->capacity; ) { + @(message_typename)__fini(&data[i]); + } + allocator.deallocate(data, allocator.state); + return false; + } + } + output->data = data; + output->size = input->size; + output->capacity = input->size; + } + for (size_t i = 0; i < input->size; ++i) { + if (!@(message_typename)__copy( + &(input->data[i]), &(output->data[i]))) + { + return false; + } + } + return true; +} diff --git a/rosidl_generator_c/resource/msg__functions.h.em b/rosidl_generator_c/resource/msg__functions.h.em index 8fa5e8eb5..15cd8b42e 100644 --- a/rosidl_generator_c/resource/msg__functions.h.em +++ b/rosidl_generator_c/resource/msg__functions.h.em @@ -69,6 +69,23 @@ ROSIDL_GENERATOR_C_PUBLIC_@(package_name) bool @(message_typename)__are_equal(const @(message_typename) * lhs, const @(message_typename) * rhs); +/// Copy a @(interface_path_to_string(interface_path)) message. +/** + * This functions performs a deep copy, as opposed to the shallow copy that + * plain assignment yields. + * + * \param[in] input The source message pointer. + * \param[out] output The target message pointer, which must + * have been initialized before calling this function. + * \return true if successful, or false if either pointer is null + * or memory allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC_@(package_name) +bool +@(message_typename)__copy( + const @(message_typename) * input, + @(message_typename) * output); + @####################################################################### @# array functions @####################################################################### @@ -131,3 +148,20 @@ void ROSIDL_GENERATOR_C_PUBLIC_@(package_name) bool @(array_typename)__are_equal(const @(array_typename) * lhs, const @(array_typename) * rhs); + +/// Copy an array of @(interface_path_to_string(interface_path)) messages. +/** + * This functions performs a deep copy, as opposed to the shallow copy that + * plain assignment yields. + * + * \param[in] input The source array pointer. + * \param[out] output The target array pointer, which must + * have been initialized before calling this function. + * \return true if successful, or false if either pointer + * is null or memory allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC_@(package_name) +bool +@(array_typename)__copy( + const @(array_typename) * input, + @(array_typename) * output); diff --git a/rosidl_generator_c/test/test_interfaces.c b/rosidl_generator_c/test/test_interfaces.c index 54688a176..ab79527e4 100644 --- a/rosidl_generator_c/test/test_interfaces.c +++ b/rosidl_generator_c/test/test_interfaces.c @@ -202,6 +202,14 @@ int test_basic_types(void) EXPECT_FALSE(rosidl_generator_c__msg__BasicTypes__are_equal(NULL, basic)); EXPECT_TRUE(rosidl_generator_c__msg__BasicTypes__are_equal(basic, basic)); + rosidl_generator_c__msg__BasicTypes * basic_copy = NULL; + basic_copy = rosidl_generator_c__msg__BasicTypes__create(); + EXPECT_NE(basic_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__BasicTypes__are_equal(basic, basic_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__BasicTypes__copy(basic, basic_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__BasicTypes__are_equal(basic, basic_copy)); + rosidl_generator_c__msg__BasicTypes__destroy(basic_copy); + rosidl_generator_c__msg__BasicTypes__destroy(basic); return 0; } @@ -250,6 +258,15 @@ int test_defaults() EXPECT_FALSE(rosidl_generator_c__msg__Defaults__are_equal(NULL, def)); EXPECT_TRUE(rosidl_generator_c__msg__Defaults__are_equal(def, def)); + rosidl_generator_c__msg__Defaults * def_copy = NULL; + def_copy = rosidl_generator_c__msg__Defaults__create(); + EXPECT_NE(def_copy, NULL); + def->bool_value = false; // mutate message to force a difference + EXPECT_FALSE(rosidl_generator_c__msg__Defaults__are_equal(def, def_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Defaults__copy(def, def_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Defaults__are_equal(def, def_copy)); + rosidl_generator_c__msg__Defaults__destroy(def_copy); + rosidl_generator_c__msg__Defaults__destroy(def); return 0; } @@ -470,6 +487,14 @@ int test_bounded_sequences() EXPECT_FALSE(rosidl_generator_c__msg__BoundedSequences__are_equal(NULL, seq)); EXPECT_TRUE(rosidl_generator_c__msg__BoundedSequences__are_equal(seq, seq)); + rosidl_generator_c__msg__BoundedSequences * seq_copy = NULL; + seq_copy = rosidl_generator_c__msg__BoundedSequences__create(); + EXPECT_NE(seq_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__BoundedSequences__are_equal(seq, seq_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__BoundedSequences__copy(seq, seq_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__BoundedSequences__are_equal(seq, seq_copy)); + rosidl_generator_c__msg__BoundedSequences__destroy(seq_copy); + rosidl_generator_c__msg__BoundedSequences__destroy(seq); return 0; } @@ -691,6 +716,14 @@ int test_unbounded_sequences() EXPECT_FALSE(rosidl_generator_c__msg__UnboundedSequences__are_equal(NULL, seq)); EXPECT_TRUE(rosidl_generator_c__msg__UnboundedSequences__are_equal(seq, seq)); + rosidl_generator_c__msg__UnboundedSequences * seq_copy = NULL; + seq_copy = rosidl_generator_c__msg__UnboundedSequences__create(); + EXPECT_NE(seq_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__UnboundedSequences__are_equal(seq, seq_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__UnboundedSequences__copy(seq, seq_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__UnboundedSequences__are_equal(seq, seq_copy)); + rosidl_generator_c__msg__UnboundedSequences__destroy(seq_copy); + rosidl_generator_c__msg__UnboundedSequences__destroy(seq); return 0; } @@ -727,6 +760,14 @@ int test_strings() EXPECT_FALSE(rosidl_generator_c__msg__Strings__are_equal(NULL, str)); EXPECT_TRUE(rosidl_generator_c__msg__Strings__are_equal(str, str)); + rosidl_generator_c__msg__Strings * str_copy = NULL; + str_copy = rosidl_generator_c__msg__Strings__create(); + EXPECT_NE(str_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__Strings__are_equal(str, str_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Strings__copy(str, str_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Strings__are_equal(str, str_copy)); + rosidl_generator_c__msg__Strings__destroy(str_copy); + rosidl_generator_c__msg__Strings__destroy(str); return 0; } @@ -770,6 +811,14 @@ int test_nested() EXPECT_FALSE(rosidl_generator_c__msg__Nested__are_equal(NULL, nested)); EXPECT_TRUE(rosidl_generator_c__msg__Nested__are_equal(nested, nested)); + rosidl_generator_c__msg__Nested * nested_copy = NULL; + nested_copy = rosidl_generator_c__msg__Nested__create(); + EXPECT_NE(nested_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__Nested__are_equal(nested, nested_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Nested__copy(nested, nested_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Nested__are_equal(nested, nested_copy)); + rosidl_generator_c__msg__Nested__destroy(nested_copy); + rosidl_generator_c__msg__Nested__destroy(nested); return 0; } @@ -1014,6 +1063,14 @@ int test_multi_nested() EXPECT_FALSE(rosidl_generator_c__msg__MultiNested__are_equal(NULL, msg)); EXPECT_TRUE(rosidl_generator_c__msg__MultiNested__are_equal(msg, msg)); + rosidl_generator_c__msg__MultiNested * msg_copy = NULL; + msg_copy = rosidl_generator_c__msg__MultiNested__create(); + EXPECT_NE(msg_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__MultiNested__are_equal(msg, msg_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__MultiNested__copy(msg, msg_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__MultiNested__are_equal(msg, msg_copy)); + rosidl_generator_c__msg__MultiNested__destroy(msg_copy); + rosidl_generator_c__msg__MultiNested__destroy(msg); return 0; } @@ -1230,6 +1287,14 @@ int test_arrays() EXPECT_FALSE(rosidl_generator_c__msg__Arrays__are_equal(NULL, arr)); EXPECT_TRUE(rosidl_generator_c__msg__Arrays__are_equal(arr, arr)); + rosidl_generator_c__msg__Arrays * arr_copy = NULL; + arr_copy = rosidl_generator_c__msg__Arrays__create(); + EXPECT_NE(arr_copy, NULL); + EXPECT_FALSE(rosidl_generator_c__msg__Arrays__are_equal(arr, arr_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Arrays__copy(arr, arr_copy)); + EXPECT_TRUE(rosidl_generator_c__msg__Arrays__are_equal(arr, arr_copy)); + rosidl_generator_c__msg__Arrays__destroy(arr_copy); + rosidl_generator_c__msg__Arrays__destroy(arr); return 0; } diff --git a/rosidl_runtime_c/include/rosidl_runtime_c/primitives_sequence_functions.h b/rosidl_runtime_c/include/rosidl_runtime_c/primitives_sequence_functions.h index 99f7f3ac0..4f8b1feaf 100644 --- a/rosidl_runtime_c/include/rosidl_runtime_c/primitives_sequence_functions.h +++ b/rosidl_runtime_c/include/rosidl_runtime_c/primitives_sequence_functions.h @@ -76,17 +76,36 @@ extern "C" const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * lhs, \ const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * rhs); +/** + * \def ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(STRUCT_NAME) + * + * \brief Copy the sequence. + * + * param input a pointer to the sequence to copy from + * param output a pointer to an initialized sequence to copy to + * return true if successful, false if either pointer is null or memory + * allocation fails. + */ +#define ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(STRUCT_NAME) \ + /** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(STRUCT_NAME) */ \ + ROSIDL_GENERATOR_C_PUBLIC \ + bool rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy( \ + const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * input, \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * output); + /** * \def ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FUNCTIONS(STRUCT_NAME) * * \brief See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_INIT(STRUCT_NAME), - * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FINI(STRUCT_NAME), and - * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME). + * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FINI(STRUCT_NAME), + * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME), and + * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(STRUCT_NAME). */ #define ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FUNCTIONS(STRUCT_NAME) \ ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_INIT(STRUCT_NAME) \ ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FINI(STRUCT_NAME) \ - ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME) + ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME) \ + ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(STRUCT_NAME) /** * \defgroup primitives_sequence_functions__basic_types Sequence functions for all basic types. @@ -126,6 +145,11 @@ ROSIDL_GENERATOR_C_PUBLIC bool rosidl_runtime_c__bool__Sequence__are_equal( const rosidl_runtime_c__boolean__Sequence * lhs, const rosidl_runtime_c__boolean__Sequence * rhs); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(bool) */ +ROSIDL_GENERATOR_C_PUBLIC +bool rosidl_runtime_c__bool__Sequence__copy( + const rosidl_runtime_c__boolean__Sequence * input, + rosidl_runtime_c__boolean__Sequence * output); /** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_INIT(byte) */ ROSIDL_GENERATOR_C_PUBLIC @@ -140,6 +164,11 @@ ROSIDL_GENERATOR_C_PUBLIC bool rosidl_runtime_c__byte__Sequence__are_equal( const rosidl_runtime_c__octet__Sequence * lhs, const rosidl_runtime_c__octet__Sequence * rhs); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(byte) */ +ROSIDL_GENERATOR_C_PUBLIC +bool rosidl_runtime_c__byte__Sequence__copy( + const rosidl_runtime_c__octet__Sequence * input, + rosidl_runtime_c__octet__Sequence * output); /** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_INIT(float32) */ ROSIDL_GENERATOR_C_PUBLIC @@ -154,6 +183,11 @@ ROSIDL_GENERATOR_C_PUBLIC bool rosidl_runtime_c__float32__Sequence__are_equal( const rosidl_runtime_c__float__Sequence * lhs, const rosidl_runtime_c__float__Sequence * rhs); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(float32) */ +ROSIDL_GENERATOR_C_PUBLIC +bool rosidl_runtime_c__float32__Sequence__copy( + const rosidl_runtime_c__float__Sequence * input, + rosidl_runtime_c__float__Sequence * output); /** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_INIT(float64) */ ROSIDL_GENERATOR_C_PUBLIC @@ -168,6 +202,11 @@ ROSIDL_GENERATOR_C_PUBLIC bool rosidl_runtime_c__float64__Sequence__are_equal( const rosidl_runtime_c__double__Sequence * lhs, const rosidl_runtime_c__double__Sequence * rhs); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_COPY(float64) */ +ROSIDL_GENERATOR_C_PUBLIC +bool rosidl_runtime_c__float64__Sequence__copy( + const rosidl_runtime_c__double__Sequence * input, + rosidl_runtime_c__double__Sequence * output); /**@}*/ #ifdef __cplusplus diff --git a/rosidl_runtime_c/include/rosidl_runtime_c/string_functions.h b/rosidl_runtime_c/include/rosidl_runtime_c/string_functions.h index ef5b6ac2b..ef553df33 100644 --- a/rosidl_runtime_c/include/rosidl_runtime_c/string_functions.h +++ b/rosidl_runtime_c/include/rosidl_runtime_c/string_functions.h @@ -55,6 +55,24 @@ ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__String__fini(rosidl_runtime_c__String * str); +/// Copy rosidl_runtime_c__String structure content. +/** + * This functions performs a deep copy, as opposed to the shallow copy that + * plain assignment yields. + * + * \param[in] input a pointer to a rosidl_runtime_c__String structure + * to copy from. + * \param[out] output a pointer to an initialized rosidl_runtime_c__String + * structure to copy into. + * \return true if successful, false if either pointer is null or memory + * allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +rosidl_runtime_c__String__copy( + const rosidl_runtime_c__String * input, + rosidl_runtime_c__String * output); + /// Check for rosidl_runtime_c__String structure equality. /** * \param[in] lhs a pointer to the left hand side of the equality operator. @@ -142,6 +160,24 @@ rosidl_runtime_c__String__Sequence__are_equal( const rosidl_runtime_c__String__Sequence * lhs, const rosidl_runtime_c__String__Sequence * rhs); +/// Copy rosidl_runtime_c__String__Sequence structure content. +/** + * This functions performs a deep copy, as opposed to the shallow copy + * that plain assignment yields. + * + * \param[in] input a pointer to a rosidl_runtime_c__String__Sequence + * structure to copy from. + * \param[out] output a pointer to an initialized rosidl_runtime_c__String__Sequence + * structure to copy into. + * \return true if successful, false if either pointer is null or memory + * allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +rosidl_runtime_c__String__Sequence__copy( + const rosidl_runtime_c__String__Sequence * input, + rosidl_runtime_c__String__Sequence * output); + /// Create a rosidl_runtime_c__String__Sequence structure with a specific size. /* * The string sequence initially has size and capacity equal to the size argument passed to the diff --git a/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h b/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h index 6ddfe8a3f..c89afabfc 100644 --- a/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h +++ b/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h @@ -67,6 +67,24 @@ rosidl_runtime_c__U16String__are_equal( const rosidl_runtime_c__U16String * lhs, const rosidl_runtime_c__U16String * rhs); +/// Copy rosidl_runtime_c__U16String structure content. +/** + * This functions performs a deep copy, as opposed to the shallow copy that + * plain assignment yields. + * + * \param[in] input a pointer to a rosidl_runtime_c__U16String structure + * to copy from. + * \param[out] output a pointer to an initialized rosidl_runtime_c__U16String + * structure to copy into. + * \return true if successful, false if either pointer is null or memory + * allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +rosidl_runtime_c__U16String__copy( + const rosidl_runtime_c__U16String * input, + rosidl_runtime_c__U16String * output); + /// Assign the uint16_t value of n characters to the rosidl_runtime_c__U16String structure. /* * This function returns `false` if memory cannot be allocated, @@ -190,6 +208,24 @@ rosidl_runtime_c__U16String__Sequence__are_equal( const rosidl_runtime_c__U16String__Sequence * lhs, const rosidl_runtime_c__U16String__Sequence * rhs); +/// Copy a U16 string sequence content. +/** + * This functions performs a deep copy, as opposed to the shallow copy + * that plain assignment yields. + * + * \param[in] input a pointer to a rosidl_runtime_c__String__Sequence + * structure to copy from. + * \param[out] output a pointer to an initialized rosidl_runtime_c__String__Sequence + * structure to copy into. + * \return true if successful, false if either pointer is null or memory + * allocation fails. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +rosidl_runtime_c__U16String__Sequence__copy( + const rosidl_runtime_c__U16String__Sequence * input, + rosidl_runtime_c__U16String__Sequence * output); + /// Create a U16 string sequence structure with a specific size. /* * The U16 string sequence initially has size and capacity equal to the size diff --git a/rosidl_runtime_c/src/primitives_sequence_functions.c b/rosidl_runtime_c/src/primitives_sequence_functions.c index 6351f9ba6..784e20773 100644 --- a/rosidl_runtime_c/src/primitives_sequence_functions.c +++ b/rosidl_runtime_c/src/primitives_sequence_functions.c @@ -15,6 +15,7 @@ #include #include #include +#include #include "rosidl_runtime_c/primitives_sequence_functions.h" @@ -74,6 +75,28 @@ } \ } \ return true; \ + } \ + \ + bool rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy( \ + const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * input, \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * output) \ + { \ + if (!input || !output) { \ + return false; \ + } \ + if (output->capacity < input->size) { \ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); \ + TYPE_NAME * data = (TYPE_NAME *)allocator.reallocate( \ + output->data, sizeof(TYPE_NAME) * input->size, allocator.state); \ + if (!data) { \ + return false; \ + } \ + output->data = data; \ + output->capacity = input->size; \ + } \ + memcpy(output->data, input->data, sizeof(TYPE_NAME) * input->size); \ + output->size = input->size; \ + return true; \ } @@ -113,6 +136,12 @@ bool rosidl_runtime_c__bool__Sequence__are_equal( { return rosidl_runtime_c__boolean__Sequence__are_equal(lhs, rhs); } +bool rosidl_runtime_c__bool__Sequence__copy( + const rosidl_runtime_c__boolean__Sequence * input, + rosidl_runtime_c__boolean__Sequence * output) +{ + return rosidl_runtime_c__boolean__Sequence__copy(input, output); +} bool rosidl_runtime_c__byte__Sequence__init( rosidl_runtime_c__octet__Sequence * sequence, size_t size) @@ -132,6 +161,12 @@ bool rosidl_runtime_c__byte__Sequence__are_equal( { return rosidl_runtime_c__octet__Sequence__are_equal(lhs, rhs); } +bool rosidl_runtime_c__byte__Sequence__copy( + const rosidl_runtime_c__octet__Sequence * input, + rosidl_runtime_c__octet__Sequence * output) +{ + return rosidl_runtime_c__octet__Sequence__copy(input, output); +} bool rosidl_runtime_c__float32__Sequence__init( rosidl_runtime_c__float__Sequence * sequence, size_t size) @@ -151,6 +186,12 @@ bool rosidl_runtime_c__float32__Sequence__are_equal( { return rosidl_runtime_c__float__Sequence__are_equal(lhs, rhs); } +bool rosidl_runtime_c__float32__Sequence__copy( + const rosidl_runtime_c__float__Sequence * input, + rosidl_runtime_c__float__Sequence * output) +{ + return rosidl_runtime_c__float__Sequence__copy(input, output); +} bool rosidl_runtime_c__float64__Sequence__init( rosidl_runtime_c__double__Sequence * sequence, size_t size) @@ -170,3 +211,9 @@ bool rosidl_runtime_c__float64__Sequence__are_equal( { return rosidl_runtime_c__double__Sequence__are_equal(lhs, rhs); } +bool rosidl_runtime_c__float64__Sequence__copy( + const rosidl_runtime_c__double__Sequence * input, + rosidl_runtime_c__double__Sequence * output) +{ + return rosidl_runtime_c__double__Sequence__copy(input, output); +} diff --git a/rosidl_runtime_c/src/string_functions.c b/rosidl_runtime_c/src/string_functions.c index cc8330be1..e8bf8cdb1 100644 --- a/rosidl_runtime_c/src/string_functions.c +++ b/rosidl_runtime_c/src/string_functions.c @@ -127,6 +127,18 @@ rosidl_runtime_c__String__assign( str, value, strlen(value)); } +bool +rosidl_runtime_c__String__copy( + const rosidl_runtime_c__String * input, + rosidl_runtime_c__String * output) +{ + if (!input) { + return false; + } + return rosidl_runtime_c__String__assignn( + output, input->data, input->size); +} + bool rosidl_runtime_c__String__Sequence__init( rosidl_runtime_c__String__Sequence * sequence, size_t size) @@ -206,6 +218,48 @@ rosidl_runtime_c__String__Sequence__are_equal( return true; } +bool +rosidl_runtime_c__String__Sequence__copy( + const rosidl_runtime_c__String__Sequence * input, + rosidl_runtime_c__String__Sequence * output) +{ + if (!input || !output) { + return false; + } + if (output->capacity < input->size) { + const size_t allocation_size = + input->size * sizeof(rosidl_runtime_c__String); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rosidl_runtime_c__String * data = + (rosidl_runtime_c__String *)allocator.reallocate( + output->data, allocation_size, allocator.state); + if (!data) { + return false; + } + for (size_t i = output->capacity; i < input->size; ++i) { + if (!rosidl_runtime_c__String__init(&data[i])) { + /* free currently allocated and return false */ + for (; i-- > output->capacity; ) { + rosidl_runtime_c__String__fini(&data[i]); + } + allocator.deallocate(data, allocator.state); + return false; + } + } + output->data = data; + output->size = input->size; + output->capacity = input->size; + } + for (size_t i = 0; i < input->size; ++i) { + if (!rosidl_runtime_c__String__copy( + &(input->data[i]), &(output->data[i]))) + { + return false; + } + } + return true; +} + rosidl_runtime_c__String__Sequence * rosidl_runtime_c__String__Sequence__create(size_t size) { diff --git a/rosidl_runtime_c/src/u16string_functions.c b/rosidl_runtime_c/src/u16string_functions.c index eae6bd3eb..88e585ec9 100644 --- a/rosidl_runtime_c/src/u16string_functions.c +++ b/rosidl_runtime_c/src/u16string_functions.c @@ -135,6 +135,18 @@ rosidl_runtime_c__U16String__assign( str, value, rosidl_runtime_c__U16String__len(value)); } +bool +rosidl_runtime_c__U16String__copy( + const rosidl_runtime_c__U16String * input, + rosidl_runtime_c__U16String * output) +{ + if (!input) { + return false; + } + return rosidl_runtime_c__U16String__assignn( + output, input->data, input->size); +} + size_t rosidl_runtime_c__U16String__len(const uint16_t * value) { @@ -249,6 +261,49 @@ rosidl_runtime_c__U16String__Sequence__are_equal( return true; } +bool +rosidl_runtime_c__U16String__Sequence__copy( + const rosidl_runtime_c__U16String__Sequence * input, + rosidl_runtime_c__U16String__Sequence * output) +{ + if (!input || !output) { + return false; + } + if (output->capacity < input->size) { + const size_t size = + input->size * sizeof(rosidl_runtime_c__U16String); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rosidl_runtime_c__U16String * data = + (rosidl_runtime_c__U16String *)allocator.reallocate( + output->data, size, allocator.state); + if (!data) { + return false; + } + for (size_t i = output->capacity; i < input->size; ++i) { + if (!rosidl_runtime_c__U16String__init(&data[i])) { + /* free currently allocated and return false */ + for (; i-- > output->capacity; ) { + rosidl_runtime_c__U16String__fini(&data[i]); + } + allocator.deallocate(data, allocator.state); + return false; + } + } + output->data = data; + output->size = input->size; + output->capacity = input->size; + } + for (size_t i = 0; i < input->size; ++i) { + if (!rosidl_runtime_c__U16String__copy( + &(input->data[i]), &(output->data[i]))) + { + return false; + } + } + return true; +} + + rosidl_runtime_c__U16String__Sequence * rosidl_runtime_c__U16String__Sequence__create(size_t size) { diff --git a/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp b/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp index bb171224f..93c916a2d 100644 --- a/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp +++ b/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp @@ -81,6 +81,23 @@ rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&nonzero_initialized); \ rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&zero_initialized); \ rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&empty); \ + } \ + \ + TEST(primitives_sequence_functions, test_ ## STRUCT_NAME ## _copy) \ + { \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence input, output; \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy(nullptr, nullptr)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy(nullptr, &output)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy(&input, nullptr)); \ + \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(&input, 1u)); \ + input.data[0] = (TYPE_NAME)1; \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(&output, 0u)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(&input, &output)); \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__copy(&input, &output)); \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(&input, &output)); \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&output); \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&input); \ } TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(float, float) diff --git a/rosidl_runtime_c/test/test_string_functions.cpp b/rosidl_runtime_c/test/test_string_functions.cpp index 20ecfcf0d..f3da91c58 100644 --- a/rosidl_runtime_c/test/test_string_functions.cpp +++ b/rosidl_runtime_c/test/test_string_functions.cpp @@ -142,6 +142,25 @@ TEST(string_functions, equality_comparison) { rosidl_runtime_c__String__fini(&empty_string); } +TEST(string_functions, copy) { + rosidl_runtime_c__String input, output; + + EXPECT_FALSE(rosidl_runtime_c__String__copy(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__copy(&input, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__copy(nullptr, &output)); + + EXPECT_TRUE(rosidl_runtime_c__String__init(&input)); + EXPECT_TRUE(rosidl_runtime_c__String__assign(&input, "foo")); + EXPECT_TRUE(rosidl_runtime_c__String__init(&output)); + + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__String__copy(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__String__are_equal(&input, &output)); + + rosidl_runtime_c__String__fini(&output); + rosidl_runtime_c__String__fini(&input); +} + TEST(string_functions, init_fini_sequence) { rosidl_runtime_c__String__Sequence sequence; EXPECT_FALSE(rosidl_runtime_c__String__Sequence__init(nullptr, 0u)); @@ -215,3 +234,22 @@ TEST(string_functions, sequence_equality_comparison) { rosidl_runtime_c__String__Sequence__fini(&empty_sequence); rosidl_runtime_c__String__Sequence__fini(&nonempty_sequence); } + +TEST(string_functions, copy_sequence) { + rosidl_runtime_c__String__Sequence input, output; + + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__copy(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__copy(&input, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__copy(nullptr, &output)); + + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__init(&input, 1u)); + EXPECT_TRUE(rosidl_runtime_c__String__assign(&input.data[0], "foo")); + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__init(&output, 0u)); + + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__copy(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__are_equal(&input, &output)); + + rosidl_runtime_c__String__Sequence__fini(&output); + rosidl_runtime_c__String__Sequence__fini(&input); +} diff --git a/rosidl_runtime_c/test/test_u16string_functions.cpp b/rosidl_runtime_c/test/test_u16string_functions.cpp index 834155945..f44088048 100644 --- a/rosidl_runtime_c/test/test_u16string_functions.cpp +++ b/rosidl_runtime_c/test/test_u16string_functions.cpp @@ -174,6 +174,26 @@ TEST(u16string_functions, equality_comparison) { rosidl_runtime_c__U16String__fini(&empty_string); } +TEST(u16string_functions, copy) { + rosidl_runtime_c__U16String input, output; + + EXPECT_FALSE(rosidl_runtime_c__U16String__copy(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__copy(&input, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__copy(nullptr, &output)); + + EXPECT_TRUE(rosidl_runtime_c__U16String__init(&input)); + const uint16_t encoded_foo_string[] = {102u, 111u, 111u, 0u}; + EXPECT_TRUE(rosidl_runtime_c__U16String__assign(&input, encoded_foo_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__init(&output)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__U16String__copy(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__U16String__are_equal(&input, &output)); + + rosidl_runtime_c__U16String__fini(&output); + rosidl_runtime_c__U16String__fini(&input); +} + TEST(u16string_functions, init_fini_sequence) { rosidl_runtime_c__U16String__Sequence sequence; EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__init(nullptr, 0u)); @@ -253,3 +273,23 @@ TEST(u16string_functions, sequence_equality_comparison) { rosidl_runtime_c__U16String__Sequence__fini(&empty_sequence); rosidl_runtime_c__U16String__Sequence__fini(&nonempty_sequence); } + +TEST(u16string_functions, copy_sequence) { + rosidl_runtime_c__U16String__Sequence input, output; + + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__copy(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__copy(&input, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__copy(nullptr, &output)); + + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__init(&input, 1u)); + const uint16_t encoded_foo_string[] = {102u, 111u, 111u, 0u}; + EXPECT_TRUE(rosidl_runtime_c__U16String__assign(&input.data[0], encoded_foo_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__init(&output, 0u)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__are_equal(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__copy(&input, &output)); + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__are_equal(&input, &output)); + + rosidl_runtime_c__U16String__Sequence__fini(&output); + rosidl_runtime_c__U16String__Sequence__fini(&input); +}