Implement copy function for C messages

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()
 {
@@ -411,3 +452,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;
+}

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);

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;
 }

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

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[out] 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[out] 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