diff --git a/rosidl_generator_c/resource/msg__functions.c.em b/rosidl_generator_c/resource/msg__functions.c.em index abcd4f08d..06b50eeee 100644 --- a/rosidl_generator_c/resource/msg__functions.c.em +++ b/rosidl_generator_c/resource/msg__functions.c.em @@ -232,6 +232,90 @@ for line in lines: }@ } +bool +@(message_typename)__are_equal(const @(message_typename) * lhs, const @(message_typename) * rhs) +{ + if (!lhs || !rhs) { + 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))__are_equal( + &(lhs->@(member.name)[i]), &(rhs->@(member.name)[i]))) + { + return false; + } +@[ else]@ + if (lhs->@(member.name)[i] != rhs->@(member.name)[i]) { + return false; + } +@[ end if]@ + } +@[ elif isinstance(member.type, AbstractSequence)]@ + if (!@(idl_type_to_c(member.type))__are_equal( + &(lhs->@(member.name)), &(rhs->@(member.name)))) + { + return false; + } +@[ elif isinstance(member.type, (AbstractGenericString, NamespacedType))]@ + if (!@(basetype_to_c(member.type))__are_equal( + &(lhs->@(member.name)), &(rhs->@(member.name)))) + { + return false; + } +@[ else]@ + if (lhs->@(member.name) != rhs->@(member.name)) { + return false; + } +@[ end if]@ +@[end for]@ + 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() { @@ -343,3 +427,60 @@ void } free(array); } + +bool +@(array_typename)__are_equal(const @(array_typename) * lhs, const @(array_typename) * rhs) +{ + if (!lhs || !rhs) { + return false; + } + if (lhs->size != rhs->size) { + return false; + } + for (size_t i = 0; i < lhs->size; ++i) { + if (!@(message_typename)__are_equal(&(lhs->data[i]), &(rhs->data[i]))) { + return false; + } + } + 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)); + @(message_typename) * data = + (@(message_typename) *)realloc(output->data, allocation_size); + 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]); + } + free(data); + return false; + } + } + output->data = data; + output->capacity = input->size; + } + output->size = 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 d0304fc6f..15cd8b42e 100644 --- a/rosidl_generator_c/resource/msg__functions.h.em +++ b/rosidl_generator_c/resource/msg__functions.h.em @@ -59,6 +59,32 @@ ROSIDL_GENERATOR_C_PUBLIC_@(package_name) void @(message_typename)__destroy(@(message_typename) * msg); +/// Check for @(interface_path_to_string(interface_path)) message equality. +/** + * \param[in] lhs The message on the left hand size of the equality operator. + * \param[in] rhs The message on the right hand size of the equality operator. + * \return true if messages are equal, otherwise false. + */ +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 @@ -112,3 +138,30 @@ ROSIDL_GENERATOR_C_PUBLIC_@(package_name) ROSIDL_GENERATOR_C_PUBLIC_@(package_name) void @(array_typename)__destroy(@(array_typename) * array); + +/// Check for @(interface_path_to_string(interface_path)) message array equality. +/** + * \param[in] lhs The message array on the left hand size of the equality operator. + * \param[in] rhs The message array on the right hand size of the equality operator. + * \return true if message arrays are equal in size and content, otherwise false. + */ +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 20547d9c3..ab79527e4 100644 --- a/rosidl_generator_c/test/test_interfaces.c +++ b/rosidl_generator_c/test/test_interfaces.c @@ -49,6 +49,14 @@ #define STRINGIFY(x) _STRINGIFY(x) #define _STRINGIFY(x) #x +#define EXPECT_FALSE(arg) if (arg) { \ + fputs(STRINGIFY(arg) " is not false\n", stderr); \ + return 1; \ +} +#define EXPECT_TRUE(arg) if (!(arg)) { \ + fputs(STRINGIFY(arg) " is not true\n", stderr); \ + return 1; \ +} #define EXPECT_EQ(arg1, arg2) if ((arg1) != (arg2)) { \ fputs(STRINGIFY(arg1) " != " STRINGIFY(arg2) "\n", stderr); \ return 1; \ @@ -189,6 +197,19 @@ int test_basic_types(void) basic->uint64_value = UINT64_MAX; EXPECT_EQ(UINT64_MAX, basic->uint64_value); + EXPECT_FALSE(rosidl_generator_c__msg__BasicTypes__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__BasicTypes__are_equal(basic, NULL)); + 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; } @@ -232,6 +253,20 @@ int test_defaults() EXPECT_EQ(-40000000, def->int64_value); EXPECT_EQ(50000000, def->uint64_value); + EXPECT_FALSE(rosidl_generator_c__msg__Defaults__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__Defaults__are_equal(def, NULL)); + 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; } @@ -447,6 +482,19 @@ int test_bounded_sequences() EXPECT_EQ(0, strcmp(seq->string_values_default.data[1].data, "max value")); EXPECT_EQ(0, strcmp(seq->string_values_default.data[2].data, "min value")); + EXPECT_FALSE(rosidl_generator_c__msg__BoundedSequences__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__BoundedSequences__are_equal(seq, NULL)); + 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; } @@ -663,6 +711,19 @@ int test_unbounded_sequences() EXPECT_EQ(0, strcmp(seq->string_values_default.data[1].data, "max value")); EXPECT_EQ(0, strcmp(seq->string_values_default.data[2].data, "min value")); + EXPECT_FALSE(rosidl_generator_c__msg__UnboundedSequences__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__UnboundedSequences__are_equal(seq, NULL)); + 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; } @@ -694,6 +755,19 @@ int test_strings() EXPECT_EQ(0, strcmp(str->bounded_string_value_default1.data, "Hello world!")); EXPECT_EQ(0, strcmp(str->bounded_string_value_default5.data, "Hello\"world!")); + EXPECT_FALSE(rosidl_generator_c__msg__Strings__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__Strings__are_equal(str, NULL)); + 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; } @@ -732,6 +806,19 @@ int test_nested() EXPECT_EQ(-40000000, nested->basic_types_value.int64_value); EXPECT_EQ(50000000, nested->basic_types_value.uint64_value); + EXPECT_FALSE(rosidl_generator_c__msg__Nested__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__Nested__are_equal(nested, NULL)); + 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; } @@ -971,6 +1058,19 @@ int test_multi_nested() EXPECT_EQ(UINT64_MAX, msg->array_of_arrays[0].uint64_values_default[2]); } + EXPECT_FALSE(rosidl_generator_c__msg__MultiNested__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__MultiNested__are_equal(msg, NULL)); + 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; } @@ -1182,6 +1282,19 @@ int test_arrays() EXPECT_EQ(0, strcmp(arr->string_values_default[1].data, "max value")); EXPECT_EQ(0, strcmp(arr->string_values_default[2].data, "min value")); + EXPECT_FALSE(rosidl_generator_c__msg__Arrays__are_equal(NULL, NULL)); + EXPECT_FALSE(rosidl_generator_c__msg__Arrays__are_equal(arr, NULL)); + 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 f82f21ead..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 @@ -61,15 +61,51 @@ extern "C" void rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini( \ rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * sequence); +/** + * \def ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME) + * + * \brief Check for sequence equality. + * + * param lhs a pointer to a sequence struct + * param rhs a pointer to another sequence struct + */ +#define ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME) \ + /** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(STRUCT_NAME) */ \ + ROSIDL_GENERATOR_C_PUBLIC \ + bool rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + 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) and - * #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_FINI(STRUCT_NAME) + * \brief See #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), 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_FINI(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. @@ -104,6 +140,16 @@ bool rosidl_runtime_c__bool__Sequence__init( ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__bool__Sequence__fini( rosidl_runtime_c__boolean__Sequence * sequence); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(bool) */ +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 @@ -113,6 +159,16 @@ bool rosidl_runtime_c__byte__Sequence__init( ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__byte__Sequence__fini( rosidl_runtime_c__octet__Sequence * sequence); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(byte) */ +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 @@ -122,6 +178,16 @@ bool rosidl_runtime_c__float32__Sequence__init( ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__float32__Sequence__fini( rosidl_runtime_c__float__Sequence * sequence); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(float32) */ +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 @@ -131,6 +197,16 @@ bool rosidl_runtime_c__float64__Sequence__init( ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__float64__Sequence__fini( rosidl_runtime_c__double__Sequence * sequence); +/** See #ROSIDL_RUNTIME_C__DECLARE_PRIMITIVE_SEQUENCE_EQ(float64) */ +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 72a0066ce..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,36 @@ 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. + * \param[in] lhs a pointer to the right hand side of the equality operator. + * \return true if rosidl_runtime_c__String structures are equal in size and content, + * otherwise false. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +rosidl_runtime_c__String__are_equal( + const rosidl_runtime_c__String * lhs, const rosidl_runtime_c__String * rhs); + /// Assign the c string pointer of n characters to the rosidl_runtime_c__String structure. /* * \param[inout] str a pointer to a string structure @@ -117,6 +147,37 @@ void rosidl_runtime_c__String__Sequence__fini( rosidl_runtime_c__String__Sequence * sequence); +/// Check for rosidl_runtime_c__String__Sequence structure equality. +/** + * \param[in] lhs a pointer to the left hand side of the equality operator. + * \param[in] lhs a pointer to the right hand side of the equality operator. + * \return true if rosidl_runtime_c__String__Sequence structures are equal + * in size and content, otherwise false. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +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 4207fc47a..c89afabfc 100644 --- a/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h +++ b/rosidl_runtime_c/include/rosidl_runtime_c/u16string_functions.h @@ -54,6 +54,37 @@ ROSIDL_GENERATOR_C_PUBLIC void rosidl_runtime_c__U16String__fini(rosidl_runtime_c__U16String * str); +/// Check for rosidl_runtime_c__U16String structure equality. +/** + * \param[in] lhs a pointer to the left hand side of the equality operator. + * \param[in] lhs a pointer to the right hand side of the equality operator. + * \return true if rosidl_runtime_c__U16String structures are equal + * in size and content, otherwise false. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +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, @@ -164,6 +195,37 @@ void rosidl_runtime_c__U16String__Sequence__fini( rosidl_runtime_c__U16String__Sequence * sequence); +/// Check for rosidl_runtime_c__U16String__Sequence structure equality. +/** + * \param[in] lhs a pointer to the left hand side of the equality operator. + * \param[in] lhs a pointer to the right hand side of the equality operator. + * \return true if rosidl_runtime_c__U16String__Sequence structures are equal + * in size and content, otherwise false. + */ +ROSIDL_GENERATOR_C_PUBLIC +bool +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 5884c8e69..94d2764ce 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" @@ -56,8 +57,48 @@ assert(0 == sequence->size); \ assert(0 == sequence->capacity); \ } \ + } \ + \ + bool rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * lhs, \ + const rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence * rhs) \ + { \ + if (!lhs || !rhs) { \ + return false; \ + } \ + if (lhs->size != rhs->size) { \ + return false; \ + } \ + for (size_t i = 0; i < lhs->size; ++i) { \ + if (lhs->data[i] != rhs->data[i]) { \ + return false; \ + } \ + } \ + 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) { \ + TYPE_NAME * data = (TYPE_NAME *)realloc( \ + output->data, sizeof(TYPE_NAME) * input->size); \ + 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; \ } + // array functions for all basic types ROSIDL_GENERATOR_C__DEFINE_PRIMITIVE_SEQUENCE_FUNCTIONS(float, float) ROSIDL_GENERATOR_C__DEFINE_PRIMITIVE_SEQUENCE_FUNCTIONS(double, double) @@ -88,6 +129,18 @@ void rosidl_runtime_c__bool__Sequence__fini( rosidl_runtime_c__boolean__Sequence__fini( sequence); } +bool rosidl_runtime_c__bool__Sequence__are_equal( + const rosidl_runtime_c__boolean__Sequence * lhs, + const rosidl_runtime_c__boolean__Sequence * rhs) +{ + 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) @@ -101,6 +154,18 @@ void rosidl_runtime_c__byte__Sequence__fini( rosidl_runtime_c__octet__Sequence__fini( sequence); } +bool rosidl_runtime_c__byte__Sequence__are_equal( + const rosidl_runtime_c__octet__Sequence * lhs, + const rosidl_runtime_c__octet__Sequence * rhs) +{ + 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) @@ -114,6 +179,18 @@ void rosidl_runtime_c__float32__Sequence__fini( rosidl_runtime_c__float__Sequence__fini( sequence); } +bool rosidl_runtime_c__float32__Sequence__are_equal( + const rosidl_runtime_c__float__Sequence * lhs, + const rosidl_runtime_c__float__Sequence * rhs) +{ + 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) @@ -127,3 +204,15 @@ void rosidl_runtime_c__float64__Sequence__fini( rosidl_runtime_c__double__Sequence__fini( sequence); } +bool rosidl_runtime_c__float64__Sequence__are_equal( + const rosidl_runtime_c__double__Sequence * lhs, + const rosidl_runtime_c__double__Sequence * rhs) +{ + 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 2e4ef3464..6efecebf7 100644 --- a/rosidl_runtime_c/src/string_functions.c +++ b/rosidl_runtime_c/src/string_functions.c @@ -74,6 +74,20 @@ rosidl_runtime_c__String__fini(rosidl_runtime_c__String * str) } } +bool +rosidl_runtime_c__String__are_equal( + const rosidl_runtime_c__String * lhs, + const rosidl_runtime_c__String * rhs) +{ + if (!lhs || !rhs) { + return false; + } + if (lhs->size != rhs->size) { + return false; + } + return memcmp(lhs->data, rhs->data, lhs->size) == 0; +} + bool rosidl_runtime_c__String__assignn( rosidl_runtime_c__String * str, const char * value, size_t n) @@ -113,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) @@ -171,6 +197,67 @@ rosidl_runtime_c__String__Sequence__fini( } } +bool +rosidl_runtime_c__String__Sequence__are_equal( + const rosidl_runtime_c__String__Sequence * lhs, + const rosidl_runtime_c__String__Sequence * rhs) +{ + if (!lhs || !rhs) { + return false; + } + if (lhs->size != rhs->size) { + return false; + } + for (size_t i = 0; i < lhs->size; ++i) { + if (!rosidl_runtime_c__String__are_equal( + &(lhs->data[i]), &(rhs->data[i]))) + { + return false; + } + } + 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); + rosidl_runtime_c__String * data = + (rosidl_runtime_c__String *)realloc(output->data, allocation_size); + 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]); + } + free(data); + return false; + } + } + output->data = data; + output->capacity = input->size; + } + output->size = 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 9ae5e4faf..b75f7b9a0 100644 --- a/rosidl_runtime_c/src/u16string_functions.c +++ b/rosidl_runtime_c/src/u16string_functions.c @@ -74,6 +74,20 @@ rosidl_runtime_c__U16String__fini(rosidl_runtime_c__U16String * str) } } +bool +rosidl_runtime_c__U16String__are_equal( + const rosidl_runtime_c__U16String * lhs, + const rosidl_runtime_c__U16String * rhs) +{ + if (!lhs || !rhs) { + return false; + } + if (lhs->size != rhs->size) { + return false; + } + return memcmp(lhs->data, rhs->data, lhs->size * sizeof(uint16_t)) == 0; +} + bool rosidl_runtime_c__U16String__assignn( rosidl_runtime_c__U16String * str, const uint16_t * value, size_t n) @@ -121,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) { @@ -214,6 +240,68 @@ rosidl_runtime_c__U16String__Sequence__fini( } } +bool +rosidl_runtime_c__U16String__Sequence__are_equal( + const rosidl_runtime_c__U16String__Sequence * lhs, + const rosidl_runtime_c__U16String__Sequence * rhs) +{ + if (!lhs || !rhs) { + return false; + } + if (lhs->size != rhs->size) { + return false; + } + for (size_t i = 0; i < lhs->size; ++i) { + if (!rosidl_runtime_c__U16String__are_equal( + &(lhs->data[i]), &(rhs->data[i]))) + { + return false; + } + } + 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); + rosidl_runtime_c__U16String * data = + (rosidl_runtime_c__U16String *)realloc(output->data, size); + 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]); + } + free(data); + return false; + } + } + output->data = data; + output->capacity = input->size; + } + output->size = 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 fe867a0d8..93c916a2d 100644 --- a/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp +++ b/rosidl_runtime_c/test/test_primitives_sequence_functions.cpp @@ -16,7 +16,7 @@ #include "rosidl_runtime_c/primitives_sequence.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" -#define TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(STRUCT_NAME) \ +#define TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(STRUCT_NAME, TYPE_NAME) \ TEST(primitives_sequence_functions, test_ ## STRUCT_NAME ## _init_fini) \ { \ EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(nullptr, 0u)); \ @@ -35,26 +35,89 @@ EXPECT_EQ(sequence.capacity, seq_size); \ EXPECT_NE(sequence.data, nullptr); \ rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__fini(&sequence); \ + } \ + \ + TEST(primitives_sequence_functions, test_ ## STRUCT_NAME ## _equality_comparison) \ + { \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence empty; \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(&empty, 0u)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(nullptr, nullptr)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(&empty, nullptr)); \ + EXPECT_FALSE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(nullptr, &empty)); \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal(&empty, &empty)); \ + \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence zero_initialized; \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(&zero_initialized, 1u)); \ + zero_initialized.data[0] = (TYPE_NAME)0; \ + EXPECT_FALSE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &zero_initialized, \ + &empty)); \ + EXPECT_FALSE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &empty, \ + &zero_initialized)); \ + EXPECT_TRUE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &zero_initialized, \ + &zero_initialized)); \ + \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence nonzero_initialized; \ + EXPECT_TRUE(rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__init(&nonzero_initialized, 1u)); \ + nonzero_initialized.data[0] = (TYPE_NAME)1; \ + EXPECT_FALSE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &nonzero_initialized, \ + &zero_initialized)); \ + EXPECT_FALSE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &zero_initialized, \ + &nonzero_initialized)); \ + EXPECT_TRUE( \ + rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence__are_equal( \ + &nonzero_initialized, \ + &nonzero_initialized)); \ + \ + 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_FUNCTION_INIT_FINI(float) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(double) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(long_double) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(char) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(wchar) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(boolean) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(octet) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(uint8) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(int8) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(uint16) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(int16) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(uint32) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(int32) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(uint64) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(int64) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(float, float) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(double, double) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(long_double, long double) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(char, signed char) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(wchar, uint16_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(boolean, bool) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(octet, uint8_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(uint8, uint8_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(int8, int8_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(uint16, uint16_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(int16, int16_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(uint32, uint32_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(int32, int32_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(uint64, uint64_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(int64, int64_t) // Testing legacy API -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(bool) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(byte) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(float32) -TEST_PRIMITIVE_SEQUENCE_FUNCTION_INIT_FINI(float64) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(bool, bool) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(byte, uint8_t) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(float32, float) +TEST_PRIMITIVE_SEQUENCE_FUNCTIONS(float64, double) diff --git a/rosidl_runtime_c/test/test_string_functions.cpp b/rosidl_runtime_c/test/test_string_functions.cpp index 0030dc4ce..f3da91c58 100644 --- a/rosidl_runtime_c/test/test_string_functions.cpp +++ b/rosidl_runtime_c/test/test_string_functions.cpp @@ -116,6 +116,51 @@ TEST(string_functions, resize_assignn) { rosidl_runtime_c__String__fini(&s); } +TEST(string_functions, equality_comparison) { + rosidl_runtime_c__String empty_string, foo_string, bar_string; + EXPECT_TRUE(rosidl_runtime_c__String__init(&empty_string)); + EXPECT_TRUE(rosidl_runtime_c__String__init(&foo_string)); + EXPECT_TRUE(rosidl_runtime_c__String__assign(&foo_string, "foo")); + EXPECT_TRUE(rosidl_runtime_c__String__init(&bar_string)); + EXPECT_TRUE(rosidl_runtime_c__String__assign(&bar_string, "bar")); + + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&empty_string, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(nullptr, &empty_string)); + EXPECT_TRUE(rosidl_runtime_c__String__are_equal(&empty_string, &empty_string)); + + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&empty_string, &foo_string)); + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&foo_string, &empty_string)); + EXPECT_TRUE(rosidl_runtime_c__String__are_equal(&foo_string, &foo_string)); + + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&bar_string, &foo_string)); + EXPECT_FALSE(rosidl_runtime_c__String__are_equal(&foo_string, &bar_string)); + EXPECT_TRUE(rosidl_runtime_c__String__are_equal(&bar_string, &bar_string)); + + rosidl_runtime_c__String__fini(&bar_string); + rosidl_runtime_c__String__fini(&foo_string); + 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)); @@ -168,3 +213,43 @@ TEST(string_functions, create_destroy_sequence_maybe_fail) { } }); } + +TEST(string_functions, sequence_equality_comparison) { + rosidl_runtime_c__String__Sequence empty_sequence; + rosidl_runtime_c__String__Sequence nonempty_sequence; + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__init(&empty_sequence, 0u)); + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__init(&nonempty_sequence, 1u)); + + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(&empty_sequence, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(nullptr, &empty_sequence)); + EXPECT_TRUE(rosidl_runtime_c__String__Sequence__are_equal(&empty_sequence, &empty_sequence)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(&empty_sequence, &nonempty_sequence)); + EXPECT_FALSE(rosidl_runtime_c__String__Sequence__are_equal(&nonempty_sequence, &empty_sequence)); + EXPECT_TRUE( + rosidl_runtime_c__String__Sequence__are_equal( + &nonempty_sequence, + &nonempty_sequence)); + + 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 b42e718ca..f44088048 100644 --- a/rosidl_runtime_c/test/test_u16string_functions.cpp +++ b/rosidl_runtime_c/test/test_u16string_functions.cpp @@ -146,6 +146,54 @@ TEST(u16string_functions, resize_assignn) { rosidl_runtime_c__U16String__fini(&s); } +TEST(u16string_functions, equality_comparison) { + rosidl_runtime_c__U16String empty_string, foo_string, bar_string; + EXPECT_TRUE(rosidl_runtime_c__U16String__init(&empty_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__init(&foo_string)); + const uint16_t encoded_foo_string[] = {102u, 111u, 111u, 0u}; + EXPECT_TRUE(rosidl_runtime_c__U16String__assign(&foo_string, encoded_foo_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__init(&bar_string)); + const uint16_t encoded_bar_string[] = {98u, 97u, 114u, 0u}; + EXPECT_TRUE(rosidl_runtime_c__U16String__assign(&bar_string, encoded_bar_string)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&empty_string, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(nullptr, &empty_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__are_equal(&empty_string, &empty_string)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&empty_string, &foo_string)); + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&foo_string, &empty_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__are_equal(&foo_string, &foo_string)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&bar_string, &foo_string)); + EXPECT_FALSE(rosidl_runtime_c__U16String__are_equal(&foo_string, &bar_string)); + EXPECT_TRUE(rosidl_runtime_c__U16String__are_equal(&bar_string, &bar_string)); + + rosidl_runtime_c__U16String__fini(&bar_string); + rosidl_runtime_c__U16String__fini(&foo_string); + 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)); @@ -197,3 +245,51 @@ TEST(string_functions, create_destroy_sequence_maybe_fail) { } }); } + + +TEST(u16string_functions, sequence_equality_comparison) { + rosidl_runtime_c__U16String__Sequence empty_sequence; + rosidl_runtime_c__U16String__Sequence nonempty_sequence; + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__init(&empty_sequence, 0u)); + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__init(&nonempty_sequence, 1u)); + + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__are_equal(nullptr, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__are_equal(&empty_sequence, nullptr)); + EXPECT_FALSE(rosidl_runtime_c__U16String__Sequence__are_equal(nullptr, &empty_sequence)); + EXPECT_TRUE(rosidl_runtime_c__U16String__Sequence__are_equal(&empty_sequence, &empty_sequence)); + EXPECT_FALSE( + rosidl_runtime_c__U16String__Sequence__are_equal( + &empty_sequence, + &nonempty_sequence)); + EXPECT_FALSE( + rosidl_runtime_c__U16String__Sequence__are_equal( + &nonempty_sequence, + &empty_sequence)); + EXPECT_TRUE( + rosidl_runtime_c__U16String__Sequence__are_equal( + &nonempty_sequence, + &nonempty_sequence)); + + 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); +}