From e3bd0a02687a3896f7070bb07a5c01d9a55a92d0 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 23 Sep 2024 21:49:38 +0000 Subject: [PATCH] Tidy proto2ros* C++ code Signed-off-by: Michel Hidalgo --- proto2ros/include/proto2ros/conversions.hpp | 70 ++++++----- .../output/templates/conversions.cpp.jinja | 62 +++++----- .../output/templates/conversions.hpp.jinja | 18 ++- proto2ros/src/conversions.cpp | 112 ++++++++++-------- .../proto2ros_tests/manual_conversions.hpp | 46 ++++--- proto2ros_tests/src/manual_conversions.cpp | 92 +++++++------- proto2ros_tests/test/test_proto2ros.cpp | 109 ++++++++--------- 7 files changed, 262 insertions(+), 247 deletions(-) diff --git a/proto2ros/include/proto2ros/conversions.hpp b/proto2ros/include/proto2ros/conversions.hpp index c9a1fcf..35ff3a3 100644 --- a/proto2ros/include/proto2ros/conversions.hpp +++ b/proto2ros/include/proto2ros/conversions.hpp @@ -29,14 +29,13 @@ #include #include -namespace proto2ros { -namespace conversions { +namespace proto2ros::conversions { /// Unpacks a proto2ros/AnyProto ROS message into any Protobuf message. /// /// \throws std::runtime_error if the given ROS message cannot be unpacked onto the given Protobuf message. template -void convert(const proto2ros::msg::AnyProto& ros_msg, T* proto_msg) { +void Convert(const proto2ros::msg::AnyProto& ros_msg, T* proto_msg) { proto_msg->Clear(); auto wrapper = google::protobuf::Any(); wrapper.set_type_url(ros_msg.type_url); @@ -50,7 +49,7 @@ void convert(const proto2ros::msg::AnyProto& ros_msg, T* proto_msg) { /// Packs any Protobuf message into a proto2ros/AnyProto ROS message. template -void convert(const T& proto_msg, proto2ros::msg::AnyProto* ros_msg) { +void Convert(const T& proto_msg, proto2ros::msg::AnyProto* ros_msg) { auto wrapper = google::protobuf::Any(); wrapper.PackFrom(proto_msg); ros_msg->type_url = wrapper.type_url(); @@ -60,94 +59,93 @@ void convert(const T& proto_msg, proto2ros::msg::AnyProto* ros_msg) { } /// Converts from proto2ros/AnyProto ROS message to google.protobuf.Any Protobuf messages. -void convert(const proto2ros::msg::AnyProto& ros_msg, google::protobuf::Any* proto_msg); +void Convert(const proto2ros::msg::AnyProto& ros_msg, google::protobuf::Any* proto_msg); /// Converts from google.protobuf.Any Protobuf messages to proto2ros/AnyProto ROS messages. -void convert(const google::protobuf::Any& proto_msg, proto2ros::msg::AnyProto* ros_msg); +void Convert(const google::protobuf::Any& proto_msg, proto2ros::msg::AnyProto* ros_msg); /// Converts from google.protobuf.Any Protobuf messages to proto2ros/AnyProto ROS messages. -void convert(const builtin_interfaces::msg::Duration& ros_msg, google::protobuf::Duration* proto_msg); +void Convert(const builtin_interfaces::msg::Duration& ros_msg, google::protobuf::Duration* proto_msg); /// Converts from google.protobuf.Duration Protobuf messages to builtin_interfaces/Duration ROS messages. -void convert(const google::protobuf::Duration& proto_msg, builtin_interfaces::msg::Duration* ros_msg); +void Convert(const google::protobuf::Duration& proto_msg, builtin_interfaces::msg::Duration* ros_msg); /// Converts from builtin_interfaces/Time ROS messages to google.protobuf.Timestamp Protobuf messages. -void convert(const builtin_interfaces::msg::Time& ros_msg, google::protobuf::Timestamp* proto_msg); +void Convert(const builtin_interfaces::msg::Time& ros_msg, google::protobuf::Timestamp* proto_msg); /// Converts from google.protobuf.Timestamp Protobuf messages to builtin_interfaces/Time ROS messages. -void convert(const google::protobuf::Timestamp& proto_msg, builtin_interfaces::msg::Time* ros_msg); +void Convert(const google::protobuf::Timestamp& proto_msg, builtin_interfaces::msg::Time* ros_msg); /// Converts from std_msgs/Float64 ROS messages to google.protobuf.DoubleValue Protobuf messages. -void convert(const std_msgs::msg::Float64& ros_msg, google::protobuf::DoubleValue* proto_msg); +void Convert(const std_msgs::msg::Float64& ros_msg, google::protobuf::DoubleValue* proto_msg); /// Converts from google.protobuf.DoubleValue Protobuf messages to std_msgs/Float64 ROS messages. -void convert(const google::protobuf::DoubleValue& proto_msg, std_msgs::msg::Float64* ros_msg); +void Convert(const google::protobuf::DoubleValue& proto_msg, std_msgs::msg::Float64* ros_msg); /// Converts from std_msgs/Float32 ROS messages to google.protobuf.FloatValue Protobuf messages. -void convert(const std_msgs::msg::Float32& ros_msg, google::protobuf::FloatValue* proto_msg); +void Convert(const std_msgs::msg::Float32& ros_msg, google::protobuf::FloatValue* proto_msg); /// Converts from google.protobuf.FloatValue Protobuf messages to std_msgs/Float32 ROS messages. -void convert(const google::protobuf::FloatValue& proto_msg, std_msgs::msg::Float32* ros_msg); +void Convert(const google::protobuf::FloatValue& proto_msg, std_msgs::msg::Float32* ros_msg); /// Converts from std_msgs/Int64 ROS messages to google.protobuf.Int64Value Protobuf messages. -void convert(const std_msgs::msg::Int64& ros_msg, google::protobuf::Int64Value* proto_msg); +void Convert(const std_msgs::msg::Int64& ros_msg, google::protobuf::Int64Value* proto_msg); /// Converts from google.protobuf.Int64Value Protobuf messages to std_msgs/Int64 ROS messages. -void convert(const google::protobuf::Int64Value& proto_msg, std_msgs::msg::Int64* ros_msg); +void Convert(const google::protobuf::Int64Value& proto_msg, std_msgs::msg::Int64* ros_msg); /// Converts from std_msgs/Int32 ROS messages to google.protobuf.Int32Value Protobuf messages. -void convert(const std_msgs::msg::Int32& ros_msg, google::protobuf::Int32Value* proto_msg); +void Convert(const std_msgs::msg::Int32& ros_msg, google::protobuf::Int32Value* proto_msg); /// Converts from google.protobuf.Int32Value Protobuf messages to std_msgs/Int32 ROS messages. -void convert(const google::protobuf::Int32Value& proto_msg, std_msgs::msg::Int32* ros_msg); +void Convert(const google::protobuf::Int32Value& proto_msg, std_msgs::msg::Int32* ros_msg); /// Converts from std_msgs/UInt64 ROS messages to google.protobuf.UInt64Value Protobuf messages. -void convert(const std_msgs::msg::UInt64& ros_msg, google::protobuf::UInt64Value* proto_msg); +void Convert(const std_msgs::msg::UInt64& ros_msg, google::protobuf::UInt64Value* proto_msg); /// Converts from google.protobuf.UInt64Value Protobuf messages to std_msgs/UInt64 ROS messages. -void convert(const google::protobuf::UInt64Value& proto_msg, std_msgs::msg::UInt64* ros_msg); +void Convert(const google::protobuf::UInt64Value& proto_msg, std_msgs::msg::UInt64* ros_msg); /// Converts from std_msgs/UInt32 ROS messages to google.protobuf.UInt32Value Protobuf messages. -void convert(const std_msgs::msg::UInt32& ros_msg, google::protobuf::UInt32Value* proto_msg); +void Convert(const std_msgs::msg::UInt32& ros_msg, google::protobuf::UInt32Value* proto_msg); /// Converts from google.protobuf.UInt32Value Protobuf messages to std_msgs/UInt32 ROS messages. -void convert(const google::protobuf::UInt32Value& proto_msg, std_msgs::msg::UInt32* ros_msg); +void Convert(const google::protobuf::UInt32Value& proto_msg, std_msgs::msg::UInt32* ros_msg); /// Converts from std_msgs/Bool ROS messages to google.protobuf.BoolValue Protobuf messages. -void convert(const std_msgs::msg::Bool& ros_msg, google::protobuf::BoolValue* proto_msg); +void Convert(const std_msgs::msg::Bool& ros_msg, google::protobuf::BoolValue* proto_msg); /// Converts from google.protobuf.BoolValue Protobuf messages to std_msgs/Bool ROS messages. -void convert(const google::protobuf::BoolValue& proto_msg, std_msgs::msg::Bool* ros_msg); +void Convert(const google::protobuf::BoolValue& proto_msg, std_msgs::msg::Bool* ros_msg); /// Converts from std_msgs/String ROS messages to google.protobuf.StringValue Protobuf messages. -void convert(const std_msgs::msg::String& ros_msg, google::protobuf::StringValue* proto_msg); +void Convert(const std_msgs::msg::String& ros_msg, google::protobuf::StringValue* proto_msg); /// Converts from google.protobuf.StringValue Protobuf messages to std_msgs/String ROS messages. -void convert(const google::protobuf::StringValue& proto_msg, std_msgs::msg::String* ros_msg); +void Convert(const google::protobuf::StringValue& proto_msg, std_msgs::msg::String* ros_msg); /// Converts from proto2ros/Bytes ROS messages to google.protobuf.BytesValue Protobuf messages. -void convert(const proto2ros::msg::Bytes& ros_msg, google::protobuf::BytesValue* proto_msg); +void Convert(const proto2ros::msg::Bytes& ros_msg, google::protobuf::BytesValue* proto_msg); /// Converts from google.protobuf.BytesValue Protobuf messages to proto2ros/Bytes ROS messages. -void convert(const google::protobuf::BytesValue& proto_msg, proto2ros::msg::Bytes* ros_msg); +void Convert(const google::protobuf::BytesValue& proto_msg, proto2ros::msg::Bytes* ros_msg); /// Converts from proto2ros/Value ROS messages to google.protobuf.Value Protobuf messages. -void convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* proto_msg); +void Convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* proto_msg); /// Converts from google.protobuf.Value Protobuf messages to proto2ros/Value ROS messages. -void convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ros_msg); +void Convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ros_msg); /// Converts from proto2ros/List ROS messages to google.protobuf.ListValue Protobuf messages. -void convert(const proto2ros::msg::List& ros_msg, google::protobuf::ListValue* proto_msg); +void Convert(const proto2ros::msg::List& ros_msg, google::protobuf::ListValue* proto_msg); /// Converts from google.protobuf.ListValue Protobuf messages to proto2ros/List ROS messages. -void convert(const google::protobuf::ListValue& proto_msg, proto2ros::msg::List* ros_msg); +void Convert(const google::protobuf::ListValue& proto_msg, proto2ros::msg::List* ros_msg); /// Converts from proto2ros/Struct ROS messages to google.protobuf.Struct Protobuf messages. -void convert(const proto2ros::msg::Struct& ros_msg, google::protobuf::Struct* proto_msg); +void Convert(const proto2ros::msg::Struct& ros_msg, google::protobuf::Struct* proto_msg); /// Converts from google.protobuf.Struct Protobuf messages to proto2ros/Struct ROS messages. -void convert(const google::protobuf::Struct& proto_msg, proto2ros::msg::Struct* ros_msg); +void Convert(const google::protobuf::Struct& proto_msg, proto2ros::msg::Struct* ros_msg); -} // namespace conversions -} // namespace proto2ros +} // namespace proto2ros::conversions diff --git a/proto2ros/proto2ros/output/templates/conversions.cpp.jinja b/proto2ros/proto2ros/output/templates/conversions.cpp.jinja index e9afc4b..c080cc9 100644 --- a/proto2ros/proto2ros/output/templates/conversions.cpp.jinja +++ b/proto2ros/proto2ros/output/templates/conversions.cpp.jinja @@ -30,7 +30,7 @@ {%- if type_spec and type_spec.annotations.get("proto-class") == "enum" -%} {#- Handle enum case i.e. extract integral values from ROS wrapper messages. -#} {%- if spec.type.is_array -%} -{{ destination }}->Reserve({{ source }}.size()); +{{ destination }}->Reserve(static_cast({{ source }}.size())); for (const auto& item : {{ source }}) { {{ destination }}->Add(static_cast<{{ type_spec.annotations["proto-type"] | as_pb2_cpp_type }}>(item.value)); } @@ -65,7 +65,7 @@ for (const auto& {{ input_item }} : {{ source }}) { } {%- else -%}{#- Handle sequence case i.e. just convert sequence items. -#} {{ destination }}->Clear(); -{{ destination }}->Reserve({{ source }}.size()); +{{ destination }}->Reserve(static_cast({{ source }}.size())); for (const auto& {{ input_item }} : {{ source }}) { auto* {{ output_item }} = {{ destination }}->Add(); {{ ros_to_proto_composite_field_code_block(input_item, output_item, spec) | indent(4) }} @@ -78,7 +78,7 @@ for (const auto& {{ input_item }} : {{ source }}) { {#- Handle bytes case. -#} {%- if to_ros_base_type(spec.type) == "proto2ros/Bytes" -%} {{ destination }}->Clear(); -{{ destination }}->Reserve({{ source }}.size()); +{{ destination }}->Reserve(static_cast({{ source }}.size())); for (const auto& blob : {{ source }}) { auto* value = {{ destination }}->Add(); value->reserve(blob.data.size()); @@ -92,9 +92,9 @@ for (const auto& blob : {{ source }}) { {%- else -%}{#- Handle primitive types case. -#} {%- if spec.type.is_array -%} {{ destination }}->Clear(); -{{ destination }}->Reserve({{ source }}.size()); +{{ destination }}->Reserve(static_cast({{ source }}.size())); for (auto item : {{ source }}) { - {{ destination }}->Add(std::move(item)); + {{ destination }}->Add(std::move(item)); // NOLINT(performance-move-const-arg) } {%- else -%} {{ destination | rreplace("mutable", "set", 1) | rreplace("()", "", 1) }}({{ source }}); @@ -129,13 +129,13 @@ for (auto item : {{ source }}) { const auto serializer = rclcpp::Serialization<{{ spec.type | as_ros_base_type | as_ros_cpp_type }}>{}; auto typed_ros_message = {{ spec.type | as_ros_base_type | as_ros_cpp_type }}(); serializer.deserialize_message(&serialized_message, &typed_ros_message); - convert(typed_ros_message, {{ destination }}); + Convert(typed_ros_message, {{ destination }}); } {%- elif spec.annotations.get("type-casted") -%} {#- ROS message must be converted and packed for assignment. -#} { auto typed_proto_message = {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}(); - convert({{ source }}, &typed_proto_message); + Convert({{ source }}, &typed_proto_message); {{ destination }}->PackFrom(typed_proto_message); } {%- elif spec.annotations.get("type-casts") -%} @@ -155,7 +155,7 @@ if ({{ source }}.type_name == "{{ ros_type_name }}") { auto typed_ros_message = {{ ros_type_name | as_ros_cpp_type }}(); serializer.deserialize_message(&serialized_message, &typed_ros_message); auto typed_proto_message = {{ proto_type_name | as_pb2_cpp_type }}(); - convert(typed_ros_message, &typed_proto_message); + Convert(typed_ros_message, &typed_proto_message); {{ destination }}->PackFrom(typed_proto_message); {%- endfor %} } else { @@ -166,7 +166,7 @@ if ({{ source }}.type_name == "{{ ros_type_name }}") { {%- elif type_spec and type_spec.annotations.get("tagged") -%} {#- Handle one-of field case i.e. determine and convert the ROS message member that is set. -#} {%- set tag_field_spec = type_spec.annotations["tag"] -%} -switch ({{ source }}.{{ tag_field_spec.name }} ? {{ source }}.{{ tag_field_spec.name }} : {{ source }}.{{ tag_field_spec.annotations["alias"] }}) { +switch ({{ source }}.{{ tag_field_spec.name }} != 0 ? {{ source }}.{{ tag_field_spec.name }} : {{ source }}.{{ tag_field_spec.annotations["alias"] }}) { {%- for tag_spec, member_spec in type_spec.annotations["tagged"] -%} {%- set source_member = source + "." + member_spec.name -%} {%- set destination_member = destination.rpartition("->")[0] + "->mutable_" + member_spec.annotations.get("proto-name", member_spec.name) + "()" %} @@ -178,7 +178,7 @@ switch ({{ source }}.{{ tag_field_spec.name }} ? {{ source }}.{{ tag_field_spec. break; } {%- else -%}{#- Handle the generic ROS message case (because it is appropriate or because we do not know any better). -#} -convert({{ source }}, {{ destination }}); +Convert({{ source }}, {{ destination }}); {%- endif -%} {%- endmacro -%} @@ -267,13 +267,13 @@ for (const auto& blob : {{ source }}) { {%- if spec.annotations.get("type-erased") -%}{#- ROS message must be serialized for assignment. -#} { auto typed_ros_message = {{ spec.type | as_ros_base_type | as_ros_cpp_type }}(); - convert({{ source }}, &typed_ros_message); + Convert({{ source }}, &typed_ros_message); const auto serde = rclcpp::Serialization<{{ spec.type | as_ros_base_type | as_ros_cpp_type }}>{}; rclcpp::SerializedMessage serialized_message; serde.serialize_message(&typed_ros_message, &serialized_message); const auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); - const auto begin = rcl_serialized_message.buffer; - const auto end = begin + rcl_serialized_message.buffer_length; + const auto* begin = rcl_serialized_message.buffer; + const auto* end = begin + rcl_serialized_message.buffer_length; {{ destination }}.value.assign(begin, end); {{ destination }}.type_name = "{{ spec.type | as_ros_base_type }}"; } @@ -284,7 +284,7 @@ for (const auto& blob : {{ source }}) { if (!{{ source }}.UnpackTo(&typed_message)) { throw std::runtime_error("Failed to unpack protobuf, internal error"); } - convert(typed_message, &{{ destination }}); + Convert(typed_message, &{{ destination }}); } {%- elif spec.annotations.get("type-casts") -%} {#- Protobuf message must be unpacked according to type, then converted, then serialized for assignment. -#} @@ -300,13 +300,13 @@ for (const auto& blob : {{ source }}) { throw std::runtime_error("Failed to unpack any protobuf, internal error"); } auto typed_ros_message = {{ ros_type_name | as_ros_cpp_type }}(); - convert(typed_protobuf_message, &typed_ros_message); + Convert(typed_protobuf_message, &typed_ros_message); const auto serializer = rclcpp::Serialization<{{ ros_type_name | as_ros_cpp_type }}>{}; rclcpp::SerializedMessage serialized_message; serializer.serialize_message(&typed_ros_message, &serialized_message); const auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); - const auto begin = rcl_serialized_message.buffer; - const auto end = begin + rcl_serialized_message.buffer_length; + const auto* begin = rcl_serialized_message.buffer; + const auto* end = begin + rcl_serialized_message.buffer_length; {{ destination }}.value.assign(begin, end); {{ destination }}.type_name = "{{ ros_type_name }}"; {%- endfor %} @@ -338,20 +338,19 @@ switch ({{ source_case }}) { break; } {%- else -%}{#- Handle the generic Protobuf message case (because it is appropriate or because we do not know any better). -#} -convert({{ source }}, &{{ destination }}); +Convert({{ source }}, &{{ destination }}); {%- endif -%} {%- endmacro %} -namespace {{ package_name }} { -namespace conversions { +namespace {{ package_name }}::conversions { -using proto2ros::conversions::convert; +using proto2ros::conversions::Convert; {%- for namespace in config.inline_cpp_namespaces %} -using {{ namespace }}::convert; +using {{ namespace }}::Convert; {%- endfor %} {% for spec in message_specifications if spec.annotations.get("proto-class") == "message" and not spec.annotations.get("map-entry") -%} -void convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}* proto_msg) { +void Convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}* proto_msg) { assert(proto_msg != nullptr); {%- if spec.fields %} proto_msg->Clear(); @@ -359,7 +358,7 @@ void convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ {%- set source = "ros_msg." + field_spec.name -%} {%- set destination = "proto_msg->mutable_" + field_spec.annotations.get("proto-name", field_spec.name).lower() + "()" -%} {%- if field_spec.annotations["optional"] %}{#- Check for field presence before use. #} - if (ros_msg.has_field & {{ spec.base_type | string | as_ros_cpp_type }}::{{ field_spec.name | upper }}_FIELD_SET) { + if ((ros_msg.has_field & {{ spec.base_type | string | as_ros_cpp_type }}::{{ field_spec.name | upper }}_FIELD_SET) != 0) { {{ ros_to_proto_field_code(source, destination, field_spec) | indent(8) }} } {%- else %} @@ -367,16 +366,16 @@ void convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ {%- endif -%} {%- endfor -%} {% else -%}{#- Handle empty message. #} - (void)ros_msg; - (void)proto_msg; + std::ignore = ros_msg; + std::ignore = proto_msg; {%- endif %} } -void convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg, {{ spec.base_type | string | as_ros_cpp_type }}* ros_msg) { +void Convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg, {{ spec.base_type | string | as_ros_cpp_type }}* ros_msg) { assert(ros_msg != nullptr); {%- if spec.fields -%} {%- if spec.annotations["has-optionals"] %} - ros_msg->has_field = 0u; + ros_msg->has_field = 0U; {%- endif -%} {%- for field_spec in spec.fields if field_spec.name != "has_field" -%} {%- set source = "proto_msg." + field_spec.annotations.get("proto-name", field_spec.name).lower() + "()" -%} @@ -391,11 +390,10 @@ void convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto {%- endif -%} {%- endfor -%} {% else -%}{#- Handle empty message. #} - (void)proto_msg; - (void)ros_msg; + std::ignore = proto_msg; + std::ignore = ros_msg; {%- endif %} } {% endfor -%} -} // namespace conversions -} // namespace {{ package_name }} +} // namespace {{ package_name }}::conversions diff --git a/proto2ros/proto2ros/output/templates/conversions.hpp.jinja b/proto2ros/proto2ros/output/templates/conversions.hpp.jinja index 4072fc0..4596f36 100644 --- a/proto2ros/proto2ros/output/templates/conversions.hpp.jinja +++ b/proto2ros/proto2ros/output/templates/conversions.hpp.jinja @@ -7,29 +7,27 @@ #include <{{ header }}> {%- endfor %} -namespace {{ package_name }} { -namespace conversions { +namespace {{ package_name }}::conversions { {% for spec in message_specifications if spec.annotations.get("proto-class") == "message" and not spec.annotations.get("map-entry") %} /// Convert from {{ spec.annotations["proto-type"] }} Protobuf messages to {{ spec.base_type }} ROS messages. -void convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg, {{ spec.base_type | string | as_ros_cpp_type }}* ros_msg); +void Convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg, {{ spec.base_type | string | as_ros_cpp_type }}* ros_msg); /// Convert from {{ spec.base_type }} ROS messages to {{ spec.annotations["proto-type"] }} Protobuf messages. -void convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}* proto_msg); +void Convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg, {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}* proto_msg); /// Convert from {{ spec.annotations["proto-type"] }} Protobuf messages to {{ spec.base_type }} ROS messages. -inline {{ spec.base_type | string | as_ros_cpp_type }} convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg) { +inline {{ spec.base_type | string | as_ros_cpp_type }} Convert(const {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}& proto_msg) { auto ros_msg = {{ spec.base_type | string | as_ros_cpp_type }}(); - convert(proto_msg, &ros_msg); + Convert(proto_msg, &ros_msg); return ros_msg; } /// Convert from {{ spec.base_type }} ROS messages to {{ spec.annotations["proto-type"] }} Protobuf messages. -inline {{ spec.annotations["proto-type"] | as_pb2_cpp_type }} convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg) { +inline {{ spec.annotations["proto-type"] | as_pb2_cpp_type }} Convert(const {{ spec.base_type | string | as_ros_cpp_type }}& ros_msg) { auto proto_msg = {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}(); - convert(ros_msg, &proto_msg); + Convert(ros_msg, &proto_msg); return proto_msg; } {% endfor -%} -} // namespace conversions -} // namespace {{ package_name }} +} // namespace {{ package_name }}::conversions diff --git a/proto2ros/src/conversions.cpp b/proto2ros/src/conversions.cpp index f92a9ed..e507a85 100644 --- a/proto2ros/src/conversions.cpp +++ b/proto2ros/src/conversions.cpp @@ -7,10 +7,9 @@ #include #include -namespace proto2ros { -namespace conversions { +namespace proto2ros::conversions { -void convert(const proto2ros::msg::AnyProto& ros_msg, google::protobuf::Any* proto_msg) { +void Convert(const proto2ros::msg::AnyProto& ros_msg, google::protobuf::Any* proto_msg) { proto_msg->Clear(); proto_msg->set_type_url(ros_msg.type_url); auto* value = proto_msg->mutable_value(); @@ -18,111 +17,123 @@ void convert(const proto2ros::msg::AnyProto& ros_msg, google::protobuf::Any* pro value->assign(ros_msg.value.begin(), ros_msg.value.end()); } -void convert(const google::protobuf::Any& proto_msg, proto2ros::msg::AnyProto* ros_msg) { +void Convert(const google::protobuf::Any& proto_msg, proto2ros::msg::AnyProto* ros_msg) { ros_msg->type_url = proto_msg.type_url(); const auto& value = proto_msg.value(); ros_msg->value.reserve(value.size()); ros_msg->value.assign(value.begin(), value.end()); } -void convert(const builtin_interfaces::msg::Duration& ros_msg, google::protobuf::Duration* proto_msg) { +void Convert(const builtin_interfaces::msg::Duration& ros_msg, google::protobuf::Duration* proto_msg) { proto_msg->set_seconds(ros_msg.sec); - proto_msg->set_nanos(ros_msg.nanosec); + if (ros_msg.nanosec > static_cast(std::numeric_limits::max())) { + throw std::range_error("builtin_interfaces/Duration does not fit in google.protobuf.Duration"); + } + proto_msg->set_nanos(static_cast(ros_msg.nanosec)); } -void convert(const google::protobuf::Duration& proto_msg, builtin_interfaces::msg::Duration* ros_msg) { - ros_msg->sec = proto_msg.seconds(); +void Convert(const google::protobuf::Duration& proto_msg, builtin_interfaces::msg::Duration* ros_msg) { + if (proto_msg.seconds() > static_cast(std::numeric_limits::max())) { + throw std::range_error("google.protobuf.Durationdoes not fit in builtin_interfaces/Duration"); + } + ros_msg->sec = static_cast(proto_msg.seconds()); ros_msg->nanosec = proto_msg.nanos(); } -void convert(const builtin_interfaces::msg::Time& ros_msg, google::protobuf::Timestamp* proto_msg) { +void Convert(const builtin_interfaces::msg::Time& ros_msg, google::protobuf::Timestamp* proto_msg) { proto_msg->set_seconds(ros_msg.sec); - proto_msg->set_nanos(ros_msg.nanosec); + if (ros_msg.nanosec > static_cast(std::numeric_limits::max())) { + throw std::range_error("builtin_interfaces/Time does not fit in google.protobuf.Timestamp"); + } + proto_msg->set_nanos(static_cast(ros_msg.nanosec)); } -void convert(const google::protobuf::Timestamp& proto_msg, builtin_interfaces::msg::Time* ros_msg) { - ros_msg->sec = proto_msg.seconds(); +void Convert(const google::protobuf::Timestamp& proto_msg, builtin_interfaces::msg::Time* ros_msg) { + if (proto_msg.seconds() > static_cast(std::numeric_limits::max())) { + throw std::range_error("google.protobuf.Timestamp does not fit in builtin_interfaces/Time"); + } + ros_msg->sec = static_cast(proto_msg.seconds()); ros_msg->nanosec = proto_msg.nanos(); } -void convert(const std_msgs::msg::Float64& ros_msg, google::protobuf::DoubleValue* proto_msg) { +void Convert(const std_msgs::msg::Float64& ros_msg, google::protobuf::DoubleValue* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::DoubleValue& proto_msg, std_msgs::msg::Float64* ros_msg) { +void Convert(const google::protobuf::DoubleValue& proto_msg, std_msgs::msg::Float64* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::Float32& ros_msg, google::protobuf::FloatValue* proto_msg) { +void Convert(const std_msgs::msg::Float32& ros_msg, google::protobuf::FloatValue* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::FloatValue& proto_msg, std_msgs::msg::Float32* ros_msg) { +void Convert(const google::protobuf::FloatValue& proto_msg, std_msgs::msg::Float32* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::Int64& ros_msg, google::protobuf::Int64Value* proto_msg) { +void Convert(const std_msgs::msg::Int64& ros_msg, google::protobuf::Int64Value* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::Int64Value& proto_msg, std_msgs::msg::Int64* ros_msg) { +void Convert(const google::protobuf::Int64Value& proto_msg, std_msgs::msg::Int64* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::Int32& ros_msg, google::protobuf::Int32Value* proto_msg) { +void Convert(const std_msgs::msg::Int32& ros_msg, google::protobuf::Int32Value* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::Int32Value& proto_msg, std_msgs::msg::Int32* ros_msg) { +void Convert(const google::protobuf::Int32Value& proto_msg, std_msgs::msg::Int32* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::UInt64& ros_msg, google::protobuf::UInt64Value* proto_msg) { +void Convert(const std_msgs::msg::UInt64& ros_msg, google::protobuf::UInt64Value* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::UInt64Value& proto_msg, std_msgs::msg::UInt64* ros_msg) { +void Convert(const google::protobuf::UInt64Value& proto_msg, std_msgs::msg::UInt64* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::UInt32& ros_msg, google::protobuf::UInt32Value* proto_msg) { +void Convert(const std_msgs::msg::UInt32& ros_msg, google::protobuf::UInt32Value* proto_msg) { proto_msg->set_value(ros_msg.data); } /// Converts from google.protobuf.UInt32Value Protobuf messages to std_msgs/UInt32 ROS messages. -void convert(const google::protobuf::UInt32Value& proto_msg, std_msgs::msg::UInt32* ros_msg) { +void Convert(const google::protobuf::UInt32Value& proto_msg, std_msgs::msg::UInt32* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::Bool& ros_msg, google::protobuf::BoolValue* proto_msg) { +void Convert(const std_msgs::msg::Bool& ros_msg, google::protobuf::BoolValue* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::BoolValue& proto_msg, std_msgs::msg::Bool* ros_msg) { +void Convert(const google::protobuf::BoolValue& proto_msg, std_msgs::msg::Bool* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const std_msgs::msg::String& ros_msg, google::protobuf::StringValue* proto_msg) { +void Convert(const std_msgs::msg::String& ros_msg, google::protobuf::StringValue* proto_msg) { proto_msg->set_value(ros_msg.data); } -void convert(const google::protobuf::StringValue& proto_msg, std_msgs::msg::String* ros_msg) { +void Convert(const google::protobuf::StringValue& proto_msg, std_msgs::msg::String* ros_msg) { ros_msg->data = proto_msg.value(); } -void convert(const proto2ros::msg::Bytes& ros_msg, google::protobuf::BytesValue* proto_msg) { +void Convert(const proto2ros::msg::Bytes& ros_msg, google::protobuf::BytesValue* proto_msg) { auto* value = proto_msg->mutable_value(); value->reserve(ros_msg.data.size()); value->assign(ros_msg.data.begin(), ros_msg.data.end()); } -void convert(const google::protobuf::BytesValue& proto_msg, proto2ros::msg::Bytes* ros_msg) { +void Convert(const google::protobuf::BytesValue& proto_msg, proto2ros::msg::Bytes* ros_msg) { const auto& value = proto_msg.value(); ros_msg->data.reserve(value.size()); ros_msg->data.assign(value.begin(), value.end()); } -void convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* proto_msg) { +void Convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* proto_msg) { proto_msg->Clear(); switch (ros_msg.kind) { case proto2ros::msg::Value::NUMBER_VALUE_SET: @@ -149,7 +160,7 @@ void convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* prot const auto serde = rclcpp::Serialization{}; auto typed_ros_message = proto2ros::msg::Struct(); serde.deserialize_message(&serialized_message, &typed_ros_message); - convert(typed_ros_message, proto_msg->mutable_struct_value()); + Convert(typed_ros_message, proto_msg->mutable_struct_value()); } break; case proto2ros::msg::Value::LIST_VALUE_SET: { if (ros_msg.list_value.type_name != "proto2ros/List") { @@ -166,7 +177,7 @@ void convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* prot const auto serde = rclcpp::Serialization{}; auto typed_ros_message = proto2ros::msg::List(); serde.deserialize_message(&serialized_message, &typed_ros_message); - convert(typed_ros_message, proto_msg->mutable_list_value()); + Convert(typed_ros_message, proto_msg->mutable_list_value()); } break; case proto2ros::msg::Value::NULL_VALUE_SET: proto_msg->set_null_value(google::protobuf::NullValue::NULL_VALUE); @@ -176,7 +187,7 @@ void convert(const proto2ros::msg::Value& ros_msg, google::protobuf::Value* prot } } -void convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ros_msg) { +void Convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ros_msg) { switch (proto_msg.kind_case()) { case google::protobuf::Value::kNumberValue: ros_msg->number_value = proto_msg.number_value(); @@ -192,26 +203,26 @@ void convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ro break; case google::protobuf::Value::kStructValue: { auto typed_ros_message = proto2ros::msg::Struct(); - convert(proto_msg.struct_value(), &typed_ros_message); + Convert(proto_msg.struct_value(), &typed_ros_message); const auto serde = rclcpp::Serialization{}; rclcpp::SerializedMessage serialized_message; serde.serialize_message(&typed_ros_message, &serialized_message); const auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); - const auto begin = rcl_serialized_message.buffer; - const auto end = begin + rcl_serialized_message.buffer_length; + const auto* begin = rcl_serialized_message.buffer; + const auto* end = begin + rcl_serialized_message.buffer_length; ros_msg->struct_value.value.assign(begin, end); ros_msg->struct_value.type_name = "proto2ros/Struct"; ros_msg->kind = proto2ros::msg::Value::STRUCT_VALUE_SET; } break; case google::protobuf::Value::kListValue: { auto typed_ros_message = proto2ros::msg::List(); - convert(proto_msg.list_value(), &typed_ros_message); + Convert(proto_msg.list_value(), &typed_ros_message); const auto serde = rclcpp::Serialization{}; rclcpp::SerializedMessage serialized_message; serde.serialize_message(&typed_ros_message, &serialized_message); const auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); - const auto begin = rcl_serialized_message.buffer; - const auto end = begin + rcl_serialized_message.buffer_length; + const auto* begin = rcl_serialized_message.buffer; + const auto* end = begin + rcl_serialized_message.buffer_length; ros_msg->list_value.value.assign(begin, end); ros_msg->list_value.type_name = "proto2ros/List"; ros_msg->kind = proto2ros::msg::Value::LIST_VALUE_SET; @@ -225,43 +236,42 @@ void convert(const google::protobuf::Value& proto_msg, proto2ros::msg::Value* ro } } -void convert(const proto2ros::msg::List& ros_msg, google::protobuf::ListValue* proto_msg) { +void Convert(const proto2ros::msg::List& ros_msg, google::protobuf::ListValue* proto_msg) { proto_msg->Clear(); auto* values = proto_msg->mutable_values(); - values->Reserve(ros_msg.values.size()); + values->Reserve(static_cast(ros_msg.values.size())); for (const auto& input_item : ros_msg.values) { auto* output_item = values->Add(); - convert(input_item, output_item); + Convert(input_item, output_item); } } -void convert(const google::protobuf::ListValue& proto_msg, proto2ros::msg::List* ros_msg) { +void Convert(const google::protobuf::ListValue& proto_msg, proto2ros::msg::List* ros_msg) { ros_msg->values.clear(); ros_msg->values.reserve(proto_msg.values_size()); for (const auto& input_item : proto_msg.values()) { auto& output_item = ros_msg->values.emplace_back(); - convert(input_item, &output_item); + Convert(input_item, &output_item); } } -void convert(const proto2ros::msg::Struct& ros_msg, google::protobuf::Struct* proto_msg) { +void Convert(const proto2ros::msg::Struct& ros_msg, google::protobuf::Struct* proto_msg) { proto_msg->Clear(); auto* output_fields = proto_msg->mutable_fields(); for (const auto& field : ros_msg.fields) { - convert(field.value, &(*output_fields)[field.key]); + Convert(field.value, &(*output_fields)[field.key]); } } -void convert(const google::protobuf::Struct& proto_msg, proto2ros::msg::Struct* ros_msg) { +void Convert(const google::protobuf::Struct& proto_msg, proto2ros::msg::Struct* ros_msg) { ros_msg->fields.clear(); const auto& input_fields = proto_msg.fields(); ros_msg->fields.reserve(input_fields.size()); for (const auto& [key, value] : input_fields) { auto& field = ros_msg->fields.emplace_back(); field.key = key; - convert(value, &field.value); + Convert(value, &field.value); } } -} // namespace conversions -} // namespace proto2ros +} // namespace proto2ros::conversions diff --git a/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp b/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp index 28d0f2d..4e2025b 100644 --- a/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp +++ b/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp @@ -13,68 +13,66 @@ #include #include -namespace proto2ros_tests { -namespace conversions { +namespace proto2ros_tests::conversions { /// Converts from geometry_msgs/Vector3 ROS messages to bosdyn.api.Vec3 Protobuf messages. -void convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Vec3* proto_msg); +void Convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Vec3* proto_msg); /// Converts from bosdyn.api.Vec3 Protobuf messages to geometry_msgs/Vector3 ROS messages. -void convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Vector3* ros_msg); +void Convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Vector3* ros_msg); /// Converts from geometry_msgs/Point ROS messages to bosdyn.api.Vec3 Protobuf messages. -void convert(const geometry_msgs::msg::Point& ros_msg, bosdyn::api::Vec3* proto_msg); +void Convert(const geometry_msgs::msg::Point& ros_msg, bosdyn::api::Vec3* proto_msg); /// Converts from bosdyn.api.Vec3 Protobuf messages to geometry_msgs/Point ROS messages. -void convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Point* ros_msg); +void Convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Point* ros_msg); /// Converts from geometry_msgs/Quaternion ROS messages to bosdyn.api.Quaternion Protobuf messages. -void convert(const geometry_msgs::msg::Quaternion& ros_msg, bosdyn::api::Quaternion* proto_msg); +void Convert(const geometry_msgs::msg::Quaternion& ros_msg, bosdyn::api::Quaternion* proto_msg); /// Converts from bosdyn.api.Quaternion Protobuf messages to geometry_msgs/Quaternion ROS messages. -void convert(const bosdyn::api::Quaternion& proto_msg, geometry_msgs::msg::Quaternion* ros_msg); +void Convert(const bosdyn::api::Quaternion& proto_msg, geometry_msgs::msg::Quaternion* ros_msg); /// Converts from geometry_msgs/Pose ROS messages to bosdyn.api.SE3Pose Protobuf messages. -void convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE3Pose* proto_msg); +void Convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE3Pose* proto_msg); /// Converts from bosdyn.api.SE3Pose Protobuf messages to geometry_msgs/Pose ROS messages. -void convert(const bosdyn::api::SE3Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg); +void Convert(const bosdyn::api::SE3Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg); /// Converts from geometry_msgs/Pose ROS messages to bosdyn.api.SE2Pose Protobuf messages. -void convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE2Pose* proto_msg); +void Convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE2Pose* proto_msg); /// Converts from bosdyn.api.SE2Pose Protobuf messages to geometry_msgs/Pose ROS messages. -void convert(const bosdyn::api::SE2Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg); +void Convert(const bosdyn::api::SE2Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg); /// Converts from geometry_msgs/Polygon ROS messages to bosdyn.api.Polygon Protobuf messages. -void convert(const geometry_msgs::msg::Polygon& ros_msg, bosdyn::api::Polygon* proto_msg); +void Convert(const geometry_msgs::msg::Polygon& ros_msg, bosdyn::api::Polygon* proto_msg); /// Converts from bosdyn.api.Polygon Protobuf messages to geometry_msgs/Polygon ROS messages. -void convert(const bosdyn::api::Polygon& proto_msg, geometry_msgs::msg::Polygon* ros_msg); +void Convert(const bosdyn::api::Polygon& proto_msg, geometry_msgs::msg::Polygon* ros_msg); /// Converts from geometry_msgs/Vector3 ROS messages to bosdyn.api.Circle Protobuf messages. -void convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Circle* proto_msg); +void Convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Circle* proto_msg); /// Converts from bosdyn.api.Circle Protobuf messages to geometry_msgs/Vector3 ROS messages. -void convert(const bosdyn::api::Circle& proto_msg, geometry_msgs::msg::Vector3* ros_msg); +void Convert(const bosdyn::api::Circle& proto_msg, geometry_msgs::msg::Vector3* ros_msg); /// Converts from geometry_msgs/Twist ROS messages to bosdyn.api.SE3Velocity Protobuf messages. -void convert(const geometry_msgs::msg::Twist& ros_msg, bosdyn::api::SE3Velocity* proto_msg); +void Convert(const geometry_msgs::msg::Twist& ros_msg, bosdyn::api::SE3Velocity* proto_msg); /// Converts from bosdyn.api.SE3Velocity Protobuf messages to geometry_msgs/Twist ROS messages. -void convert(const bosdyn::api::SE3Velocity& proto_msg, geometry_msgs::msg::Twist* ros_msg); +void Convert(const bosdyn::api::SE3Velocity& proto_msg, geometry_msgs::msg::Twist* ros_msg); /// Converts from geometry_msgs/Wrench ROS messages to bosdyn.api.Wrench Protobuf messages. -void convert(const geometry_msgs::msg::Wrench& ros_msg, bosdyn::api::Wrench* proto_msg); +void Convert(const geometry_msgs::msg::Wrench& ros_msg, bosdyn::api::Wrench* proto_msg); /// Converts from bosdyn.api.Wrench Protobuf messages to geometry_msgs/Wrench ROS messages. -void convert(const bosdyn::api::Wrench& proto_msg, geometry_msgs::msg::Wrench* ros_msg); +void Convert(const bosdyn::api::Wrench& proto_msg, geometry_msgs::msg::Wrench* ros_msg); /// Converts from sensor_msgs/Temperature ROS messages to proto2ros_tests.Temperature Protobuf messages. -void convert(const sensor_msgs::msg::Temperature& ros_msg, Temperature* proto_msg); +void Convert(const sensor_msgs::msg::Temperature& ros_msg, Temperature* proto_msg); /// Converts from proto2ros_tests.Temperature Protobuf messages to sensor_msgs/Temperature ROS messages. -void convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_msg); +void Convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_msg); -} // namespace conversions -} // namespace proto2ros_tests +} // namespace proto2ros_tests::conversions diff --git a/proto2ros_tests/src/manual_conversions.cpp b/proto2ros_tests/src/manual_conversions.cpp index da659a4..09d9f48 100644 --- a/proto2ros_tests/src/manual_conversions.cpp +++ b/proto2ros_tests/src/manual_conversions.cpp @@ -4,74 +4,85 @@ #include -namespace proto2ros_tests { -namespace conversions { +namespace proto2ros_tests::conversions { -void convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Vec3* proto_msg) { +void Convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Vec3* proto_msg) { proto_msg->set_x(ros_msg.x); proto_msg->set_y(ros_msg.y); proto_msg->set_z(ros_msg.z); } -void convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Vector3* ros_msg) { +void Convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Vector3* ros_msg) { ros_msg->x = proto_msg.x(); ros_msg->y = proto_msg.y(); ros_msg->z = proto_msg.z(); } -void convert(const geometry_msgs::msg::Point& ros_msg, bosdyn::api::Vec3* proto_msg) { +void Convert(const geometry_msgs::msg::Point& ros_msg, bosdyn::api::Vec3* proto_msg) { proto_msg->set_x(ros_msg.x); proto_msg->set_y(ros_msg.y); proto_msg->set_z(ros_msg.z); } -void convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Point* ros_msg) { +void Convert(const bosdyn::api::Vec3& proto_msg, geometry_msgs::msg::Point* ros_msg) { ros_msg->x = proto_msg.x(); ros_msg->y = proto_msg.y(); ros_msg->z = proto_msg.z(); } -void convert(const geometry_msgs::msg::Quaternion& ros_msg, bosdyn::api::Quaternion* proto_msg) { +void Convert(const geometry_msgs::msg::Quaternion& ros_msg, bosdyn::api::Quaternion* proto_msg) { proto_msg->set_x(ros_msg.x); proto_msg->set_y(ros_msg.y); proto_msg->set_z(ros_msg.z); proto_msg->set_w(ros_msg.w); } -void convert(const bosdyn::api::Quaternion& proto_msg, geometry_msgs::msg::Quaternion* ros_msg) { +void Convert(const bosdyn::api::Quaternion& proto_msg, geometry_msgs::msg::Quaternion* ros_msg) { ros_msg->x = proto_msg.x(); ros_msg->y = proto_msg.y(); ros_msg->z = proto_msg.z(); ros_msg->w = proto_msg.w(); } -void convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE3Pose* proto_msg) { - convert(ros_msg.position, proto_msg->mutable_position()); - convert(ros_msg.orientation, proto_msg->mutable_rotation()); +void Convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE3Pose* proto_msg) { + Convert(ros_msg.position, proto_msg->mutable_position()); + Convert(ros_msg.orientation, proto_msg->mutable_rotation()); } -void convert(const bosdyn::api::SE3Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg) { - convert(proto_msg.position(), &ros_msg->position); - convert(proto_msg.rotation(), &ros_msg->orientation); +void Convert(const bosdyn::api::SE3Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg) { + Convert(proto_msg.position(), &ros_msg->position); + Convert(proto_msg.rotation(), &ros_msg->orientation); } -void convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE2Pose* proto_msg) { +void Convert(const geometry_msgs::msg::Pose& ros_msg, bosdyn::api::SE2Pose* proto_msg) { + // NOLINTBEGIN(readability-magic-numbers) + proto_msg->Clear(); proto_msg->mutable_position()->set_x(ros_msg.position.x); proto_msg->mutable_position()->set_y(ros_msg.position.y); proto_msg->set_angle(2.0 * std::acos(ros_msg.orientation.w)); + const double angle = 2.0 * std::asin(ros_msg.orientation.z); + if (std::abs(proto_msg->angle() - angle) > std::numeric_limits::epsilon()) { + throw std::domain_error("geometry_msgs/Pose does not represent a bosdyn.api.SE2Pose"); + } + // NOLINTEND(readability-magic-numbers) } -void convert(const bosdyn::api::SE2Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg) { +void Convert(const bosdyn::api::SE2Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg) { + // NOLINTBEGIN(readability-magic-numbers) ros_msg->position.x = proto_msg.position().x(); ros_msg->position.y = proto_msg.position().y(); + ros_msg->position.z = 0.0; ros_msg->orientation.w = std::cos(proto_msg.angle() / 2.0); ros_msg->orientation.z = std::sin(proto_msg.angle() / 2.0); + ros_msg->orientation.y = 0.0; + ros_msg->orientation.x = 0.0; + // NOLINTEND(readability-magic-numbers) } -void convert(const geometry_msgs::msg::Polygon& ros_msg, bosdyn::api::Polygon* proto_msg) { +void Convert(const geometry_msgs::msg::Polygon& ros_msg, bosdyn::api::Polygon* proto_msg) { proto_msg->Clear(); auto* vertices = proto_msg->mutable_vertexes(); - vertices->Reserve(ros_msg.points.size()); + vertices->Reserve(static_cast(ros_msg.points.size())); for (const auto& point : ros_msg.points) { auto* vertex = vertices->Add(); vertex->set_x(point.x); @@ -79,56 +90,57 @@ void convert(const geometry_msgs::msg::Polygon& ros_msg, bosdyn::api::Polygon* p } } -void convert(const bosdyn::api::Polygon& proto_msg, geometry_msgs::msg::Polygon* ros_msg) { +void Convert(const bosdyn::api::Polygon& proto_msg, geometry_msgs::msg::Polygon* ros_msg) { ros_msg->points.clear(); ros_msg->points.reserve(proto_msg.vertexes_size()); for (const auto& vertex : proto_msg.vertexes()) { auto& point = ros_msg->points.emplace_back(); - point.x = vertex.x(); - point.y = vertex.y(); + point.x = static_cast(vertex.x()); + point.y = static_cast(vertex.y()); } } -void convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Circle* proto_msg) { +void Convert(const geometry_msgs::msg::Vector3& ros_msg, bosdyn::api::Circle* proto_msg) { auto* center = proto_msg->mutable_center_pt(); center->set_x(ros_msg.x); center->set_y(ros_msg.y); proto_msg->set_radius(ros_msg.z); } -void convert(const bosdyn::api::Circle& proto_msg, geometry_msgs::msg::Vector3* ros_msg) { +void Convert(const bosdyn::api::Circle& proto_msg, geometry_msgs::msg::Vector3* ros_msg) { const auto& center = proto_msg.center_pt(); ros_msg->x = center.x(); ros_msg->y = center.y(); ros_msg->z = proto_msg.radius(); } -void convert(const geometry_msgs::msg::Twist& ros_msg, bosdyn::api::SE3Velocity* proto_msg) { - convert(ros_msg.linear, proto_msg->mutable_linear()); - convert(ros_msg.angular, proto_msg->mutable_angular()); +void Convert(const geometry_msgs::msg::Twist& ros_msg, bosdyn::api::SE3Velocity* proto_msg) { + Convert(ros_msg.linear, proto_msg->mutable_linear()); + Convert(ros_msg.angular, proto_msg->mutable_angular()); } -void convert(const bosdyn::api::SE3Velocity& proto_msg, geometry_msgs::msg::Twist* ros_msg) { - convert(proto_msg.linear(), &ros_msg->linear); - convert(proto_msg.angular(), &ros_msg->angular); +void Convert(const bosdyn::api::SE3Velocity& proto_msg, geometry_msgs::msg::Twist* ros_msg) { + Convert(proto_msg.linear(), &ros_msg->linear); + Convert(proto_msg.angular(), &ros_msg->angular); } -void convert(const geometry_msgs::msg::Wrench& ros_msg, bosdyn::api::Wrench* proto_msg) { - convert(ros_msg.force, proto_msg->mutable_force()); - convert(ros_msg.torque, proto_msg->mutable_torque()); +void Convert(const geometry_msgs::msg::Wrench& ros_msg, bosdyn::api::Wrench* proto_msg) { + Convert(ros_msg.force, proto_msg->mutable_force()); + Convert(ros_msg.torque, proto_msg->mutable_torque()); } -void convert(const bosdyn::api::Wrench& proto_msg, geometry_msgs::msg::Wrench* ros_msg) { - convert(proto_msg.force(), &ros_msg->force); - convert(proto_msg.torque(), &ros_msg->torque); +void Convert(const bosdyn::api::Wrench& proto_msg, geometry_msgs::msg::Wrench* ros_msg) { + Convert(proto_msg.force(), &ros_msg->force); + Convert(proto_msg.torque(), &ros_msg->torque); } -void convert(const sensor_msgs::msg::Temperature& ros_msg, Temperature* proto_msg) { +void Convert(const sensor_msgs::msg::Temperature& ros_msg, Temperature* proto_msg) { proto_msg->set_scale(Temperature::CELSIUS); - proto_msg->set_value(ros_msg.temperature); + proto_msg->set_value(static_cast(ros_msg.temperature)); } -void convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_msg) { +void Convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_msg) { + // NOLINTBEGIN(readability-magic-numbers) switch (proto_msg.scale()) { case Temperature::KELVIN: ros_msg->temperature = proto_msg.value() + 273.0; @@ -141,7 +153,7 @@ void convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_ms ros_msg->temperature = proto_msg.value(); break; } + // NOLINTEND(readability-magic-numbers) } -} // namespace conversions -} // namespace proto2ros_tests +} // namespace proto2ros_tests::conversions diff --git a/proto2ros_tests/test/test_proto2ros.cpp b/proto2ros_tests/test/test_proto2ros.cpp index 665090d..7335a0d 100644 --- a/proto2ros_tests/test/test_proto2ros.cpp +++ b/proto2ros_tests/test/test_proto2ros.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. - +// NOLINTBEGIN(readability-magic-numbers,readability-function-cognitive-complexity,cppcoreguidelines-pro-type-reinterpret-cast) #include #include #include @@ -19,8 +19,8 @@ #include #include -using proto2ros::conversions::convert; -using proto2ros_tests::conversions::convert; +using proto2ros::conversions::Convert; +using proto2ros_tests::conversions::Convert; TEST(Proto2RosTesting, MessageMapping) { auto proto_request = proto2ros_tests::HVACControlRequest(); @@ -30,10 +30,10 @@ TEST(Proto2RosTesting, MessageMapping) { setpoint->set_scale(proto2ros_tests::Temperature::FAHRENHEIT); auto ros_request = proto2ros_tests::msg::HVACControlRequest(); - convert(proto_request, &ros_request); + Convert(proto_request, &ros_request); auto other_proto_request = proto2ros_tests::HVACControlRequest(); - convert(ros_request, &other_proto_request); + Convert(ros_request, &other_proto_request); EXPECT_DOUBLE_EQ(proto_request.air_flow_rate(), other_proto_request.air_flow_rate()); // sensor_msgs/Temperature messages are in the Celsius temperature scale @@ -48,7 +48,7 @@ TEST(Proto2RosTesting, RecursiveMessages) { proto_subfragment->set_payload("important addendum"); auto ros_fragment = proto2ros_tests::msg::Fragment(); - convert(proto_fragment, &ros_fragment); + Convert(proto_fragment, &ros_fragment); const auto payload = std::string_view{reinterpret_cast(ros_fragment.payload.data()), ros_fragment.payload.size()}; @@ -56,7 +56,7 @@ TEST(Proto2RosTesting, RecursiveMessages) { EXPECT_EQ(ros_fragment.nested.size(), static_cast(proto_fragment.nested_size())); auto other_proto_fragment = proto2ros_tests::Fragment(); - convert(ros_fragment, &other_proto_fragment); + Convert(ros_fragment, &other_proto_fragment); EXPECT_EQ(other_proto_fragment.payload(), proto_fragment.payload()); ASSERT_EQ(other_proto_fragment.nested_size(), proto_fragment.nested_size()); const auto& other_proto_subfragment = other_proto_fragment.nested(0); @@ -67,18 +67,18 @@ TEST(Proto2RosTesting, CircularlyDependentMessages) { auto proto_value = proto2ros_tests::Value(); auto* proto_items = proto_value.mutable_dict()->mutable_items(); auto* proto_pair = (*proto_items)["interval"].mutable_pair(); - proto_pair->mutable_first()->set_number(-0.5); - proto_pair->mutable_second()->set_number(0.5); + proto_pair->mutable_first()->set_number(-0.5F); + proto_pair->mutable_second()->set_number(0.5F); auto* proto_list = (*proto_items)["range"].mutable_list(); - for (double number : {-0.1, 0.0, 0.1, -0.7, 0.3, 0.4}) { + for (const float number : {-0.1F, 0.0F, 0.1F, -0.7F, 0.3F, 0.4F}) { proto_list->add_values()->set_number(number); } auto ros_value = proto2ros_tests::msg::Value(); - convert(proto_value, &ros_value); + Convert(proto_value, &ros_value); auto other_proto_value = proto2ros_tests::Value(); - convert(ros_value, &other_proto_value); + Convert(ros_value, &other_proto_value); const auto& other_proto_items = other_proto_value.dict().items(); EXPECT_TRUE(other_proto_items.count("interval")); @@ -98,12 +98,12 @@ TEST(Proto2RosTesting, MessagesWithEnums) { proto_motion_request.set_direction(proto2ros_tests::MotionRequest::Forward); proto_motion_request.set_speed(1.0); auto ros_motion_request = proto2ros_tests::msg::MotionRequest(); - convert(proto_motion_request, &ros_motion_request); + Convert(proto_motion_request, &ros_motion_request); EXPECT_EQ(ros_motion_request.direction.value, proto_motion_request.direction()); EXPECT_EQ(ros_motion_request.speed, proto_motion_request.speed()); auto other_proto_motion_request = proto2ros_tests::MotionRequest(); - convert(ros_motion_request, &other_proto_motion_request); + Convert(ros_motion_request, &other_proto_motion_request); EXPECT_EQ(other_proto_motion_request.direction(), proto_motion_request.direction()); EXPECT_EQ(other_proto_motion_request.speed(), proto_motion_request.speed()); } @@ -114,12 +114,12 @@ TEST(Proto2RosTesting, MessagesWithNestedEnums) { proto_http_request.set_uri("https://proto2ros.xyz/post"); auto ros_http_request = proto2ros_tests::msg::HTTPRequest(); - convert(proto_http_request, &ros_http_request); + Convert(proto_http_request, &ros_http_request); EXPECT_EQ(ros_http_request.method.value, proto_http_request.method()); EXPECT_EQ(ros_http_request.uri, proto_http_request.uri()); auto other_proto_http_request = proto2ros_tests::HTTP::Request(); - convert(ros_http_request, &other_proto_http_request); + Convert(ros_http_request, &other_proto_http_request); EXPECT_EQ(other_proto_http_request.method(), proto_http_request.method()); EXPECT_EQ(other_proto_http_request.uri(), proto_http_request.uri()); } @@ -130,15 +130,15 @@ TEST(Proto2RosTesting, OneOfMessages) { proto_any_command.mutable_walk()->set_speed(1.0); auto ros_any_command = proto2ros_tests::msg::AnyCommand(); - convert(proto_any_command, &ros_any_command); - constexpr auto kExpectedChoice = proto2ros_tests::msg::AnyCommandOneOfCommands::COMMANDS_WALK_SET; - EXPECT_EQ(ros_any_command.commands.which, kExpectedChoice); - EXPECT_EQ(ros_any_command.commands.commands_choice, kExpectedChoice); + Convert(proto_any_command, &ros_any_command); + constexpr auto expected_choice = proto2ros_tests::msg::AnyCommandOneOfCommands::COMMANDS_WALK_SET; + EXPECT_EQ(ros_any_command.commands.which, expected_choice); + EXPECT_EQ(ros_any_command.commands.commands_choice, expected_choice); EXPECT_EQ(ros_any_command.commands.walk.distance, proto_any_command.walk().distance()); EXPECT_EQ(ros_any_command.commands.walk.speed, proto_any_command.walk().speed()); auto other_proto_any_command = proto2ros_tests::AnyCommand(); - convert(ros_any_command, &other_proto_any_command); + Convert(ros_any_command, &other_proto_any_command); EXPECT_TRUE(other_proto_any_command.has_walk()); EXPECT_EQ(other_proto_any_command.walk().distance(), proto_any_command.walk().distance()); EXPECT_EQ(other_proto_any_command.walk().speed(), proto_any_command.walk().speed()); @@ -149,14 +149,14 @@ TEST(Proto2RosTesting, OneOfEmptyMessages) { (void)proto_any_command.mutable_sit(); auto ros_any_command = proto2ros_tests::msg::AnyCommand(); - convert(proto_any_command, &ros_any_command); + Convert(proto_any_command, &ros_any_command); - constexpr auto kExpectedChoice = proto2ros_tests::msg::AnyCommandOneOfCommands::COMMANDS_SIT_SET; - EXPECT_EQ(ros_any_command.commands.which, kExpectedChoice); - EXPECT_EQ(ros_any_command.commands.commands_choice, kExpectedChoice); + constexpr auto expected_choice = proto2ros_tests::msg::AnyCommandOneOfCommands::COMMANDS_SIT_SET; + EXPECT_EQ(ros_any_command.commands.which, expected_choice); + EXPECT_EQ(ros_any_command.commands.commands_choice, expected_choice); auto other_proto_any_command = proto2ros_tests::AnyCommand(); - convert(ros_any_command, &other_proto_any_command); + Convert(ros_any_command, &other_proto_any_command); EXPECT_TRUE(other_proto_any_command.has_sit()); } @@ -168,7 +168,7 @@ TEST(Proto2RosTesting, MessagesWithMapField) { proto_attributes["origin"] = "localization subsystem"; auto ros_diagnostic = proto2ros_tests::msg::Diagnostic(); - convert(proto_diagnostic, &ros_diagnostic); + Convert(proto_diagnostic, &ros_diagnostic); EXPECT_EQ(ros_diagnostic.severity.value, proto_diagnostic.severity()); EXPECT_EQ(ros_diagnostic.reason, proto_diagnostic.reason()); EXPECT_EQ(ros_diagnostic.reason, proto_diagnostic.reason()); @@ -177,7 +177,7 @@ TEST(Proto2RosTesting, MessagesWithMapField) { EXPECT_EQ(ros_diagnostic.attributes[0].value, "localization subsystem"); auto other_proto_diagnostic = proto2ros_tests::Diagnostic(); - convert(ros_diagnostic, &other_proto_diagnostic); + Convert(ros_diagnostic, &other_proto_diagnostic); EXPECT_EQ(other_proto_diagnostic.severity(), proto_diagnostic.severity()); EXPECT_EQ(other_proto_diagnostic.reason(), proto_diagnostic.reason()); EXPECT_TRUE(other_proto_diagnostic.attributes().contains("origin")); @@ -191,13 +191,13 @@ TEST(Proto2RosTesting, MessagesWithDeprecatedFields) { proto_remote_execution_request.set_timeout_sec(10.0); auto ros_remote_execution_request = proto2ros_tests::msg::RemoteExecutionRequest(); - convert(proto_remote_execution_request, &ros_remote_execution_request); + Convert(proto_remote_execution_request, &ros_remote_execution_request); EXPECT_EQ(ros_remote_execution_request.prompt, proto_remote_execution_request.prompt()); EXPECT_EQ(ros_remote_execution_request.timeout, proto_remote_execution_request.timeout()); EXPECT_EQ(ros_remote_execution_request.timeout_sec, proto_remote_execution_request.timeout_sec()); auto other_proto_remote_execution_request = proto2ros_tests::RemoteExecutionRequest(); - convert(ros_remote_execution_request, &other_proto_remote_execution_request); + Convert(ros_remote_execution_request, &other_proto_remote_execution_request); EXPECT_EQ(other_proto_remote_execution_request.prompt(), proto_remote_execution_request.prompt()); EXPECT_EQ(other_proto_remote_execution_request.timeout(), proto_remote_execution_request.timeout()); EXPECT_EQ(other_proto_remote_execution_request.timeout_sec(), proto_remote_execution_request.timeout_sec()); @@ -212,7 +212,7 @@ TEST(Proto2RosTesting, RedefinedMessages) { proto_remote_execution_error->add_traceback(""); auto ros_remote_execution_result = proto2ros_tests::msg::RemoteExecutionResult(); - convert(proto_remote_execution_result, &ros_remote_execution_result); + Convert(proto_remote_execution_result, &ros_remote_execution_result); EXPECT_EQ(ros_remote_execution_result.ok, proto_remote_execution_result.ok()); EXPECT_EQ(ros_remote_execution_result.error.code, proto_remote_execution_result.error().code()); EXPECT_EQ(ros_remote_execution_result.error.reason, proto_remote_execution_result.error().reason()); @@ -221,7 +221,7 @@ TEST(Proto2RosTesting, RedefinedMessages) { EXPECT_EQ(ros_remote_execution_result.error.traceback[0], proto_remote_execution_result.error().traceback(0)); auto other_proto_remote_execution_result = proto2ros_tests::RemoteExecutionResult(); - convert(ros_remote_execution_result, &other_proto_remote_execution_result); + Convert(ros_remote_execution_result, &other_proto_remote_execution_result); EXPECT_EQ(other_proto_remote_execution_result.ok(), proto_remote_execution_result.ok()); EXPECT_EQ(other_proto_remote_execution_result.error().code(), proto_remote_execution_result.error().code()); EXPECT_EQ(other_proto_remote_execution_result.error().reason(), proto_remote_execution_result.error().reason()); @@ -232,7 +232,7 @@ TEST(Proto2RosTesting, RedefinedMessages) { } namespace { - +// NOLINTBEGIN(readability-identifier-naming) template struct has_rotation_member : std::false_type {}; @@ -242,7 +242,7 @@ struct has_rotation_member().rotation)>> template constexpr bool has_rotation_member_v = has_rotation_member::value; - +// NOLINTEND(readability-identifier-naming) } // namespace TEST(Proto2RosTesting, MessagesWithReservedFields) { @@ -252,14 +252,14 @@ TEST(Proto2RosTesting, MessagesWithReservedFields) { proto_displacement.mutable_translation()->set_z(3.0); auto ros_displacement = proto2ros_tests::msg::Displacement(); - convert(proto_displacement, &ros_displacement); + Convert(proto_displacement, &ros_displacement); EXPECT_EQ(ros_displacement.translation.x, proto_displacement.translation().x()); EXPECT_EQ(ros_displacement.translation.y, proto_displacement.translation().y()); EXPECT_EQ(ros_displacement.translation.z, proto_displacement.translation().z()); static_assert(!has_rotation_member_v); auto other_proto_displacement = proto2ros_tests::Displacement(); - convert(ros_displacement, &other_proto_displacement); + Convert(ros_displacement, &other_proto_displacement); EXPECT_EQ(other_proto_displacement.translation().x(), proto_displacement.translation().x()); EXPECT_EQ(other_proto_displacement.translation().y(), proto_displacement.translation().y()); EXPECT_EQ(other_proto_displacement.translation().z(), proto_displacement.translation().z()); @@ -271,26 +271,26 @@ TEST(Proto2RosTesting, MessageForwarding) { proto_camera_info.set_width(1280); proto_camera_info.mutable_k()->set_rows(3); proto_camera_info.mutable_k()->set_cols(3); - for (double number : {2000.0, 0.0, 800.0, 0.0, 2000.0, 800.0, 0.0, 0.0, 1.0}) { + for (const double number : {2000.0, 0.0, 800.0, 0.0, 2000.0, 800.0, 0.0, 0.0, 1.0}) { proto_camera_info.mutable_k()->add_data(number); } proto_camera_info.mutable_r()->set_rows(3); proto_camera_info.mutable_r()->set_cols(3); - for (double number : {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}) { + for (const double number : {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}) { proto_camera_info.mutable_r()->add_data(number); } proto_camera_info.mutable_p()->set_rows(3); proto_camera_info.mutable_p()->set_cols(4); - for (double number : {2000.0, 0.0, 800.0, 0.0, 0.0, 2000.0, 800.0, 0.0, 0.0, 0.0, 0.0, 1.0}) { + for (const double number : {2000.0, 0.0, 800.0, 0.0, 0.0, 2000.0, 800.0, 0.0, 0.0, 0.0, 0.0, 1.0}) { proto_camera_info.mutable_p()->add_data(number); } proto_camera_info.mutable_distortion_model()->set_type(proto2ros_tests::CameraInfo::DistortionModel::PLUMB_BOB); - for (double coeff : {-0.2, -0.4, -0.0001, -0.0001, 0.0}) { + for (const double coeff : {-0.2, -0.4, -0.0001, -0.0001, 0.0}) { proto_camera_info.mutable_distortion_model()->add_coefficients(coeff); } auto ros_camera_info = proto2ros_tests::msg::CameraInfo(); - convert(proto_camera_info, &ros_camera_info); + Convert(proto_camera_info, &ros_camera_info); EXPECT_EQ(ros_camera_info.height, proto_camera_info.height()); EXPECT_EQ(ros_camera_info.width, proto_camera_info.width()); EXPECT_EQ(ros_camera_info.k.rows, proto_camera_info.k().rows()); @@ -319,7 +319,7 @@ TEST(Proto2RosTesting, MessageForwarding) { } auto other_proto_camera_info = proto2ros_tests::CameraInfo(); - convert(ros_camera_info, &other_proto_camera_info); + Convert(ros_camera_info, &other_proto_camera_info); EXPECT_EQ(other_proto_camera_info.height(), proto_camera_info.height()); EXPECT_EQ(other_proto_camera_info.width(), proto_camera_info.width()); EXPECT_EQ(other_proto_camera_info.k().rows(), proto_camera_info.k().rows()); @@ -356,10 +356,10 @@ TEST(Proto2RosTesting, MessagesWithSubMessageMapField) { proto_fragment.set_height(20); proto_fragment.set_width(20); auto* proto_grid = proto_fragment.mutable_grid(); - proto_grid->resize(20 * 20, '\0'); + proto_grid->resize(20UL * 20UL, '\0'); auto ros_map = proto2ros_tests::msg::Map(); - convert(proto_map, &ros_map); + Convert(proto_map, &ros_map); EXPECT_EQ(ros_map.submaps.size(), static_cast(proto_submaps.size())); EXPECT_EQ(ros_map.submaps[0].key, 13); auto& ros_fragment = ros_map.submaps[0].value; @@ -370,7 +370,7 @@ TEST(Proto2RosTesting, MessagesWithSubMessageMapField) { EXPECT_EQ(grid, proto_fragment.grid()); auto other_proto_map = proto2ros_tests::Map(); - convert(ros_map, &other_proto_map); + Convert(ros_map, &other_proto_map); EXPECT_TRUE(other_proto_map.submaps().contains(13)); const auto& other_proto_fragment = other_proto_map.submaps().at(13); EXPECT_EQ(other_proto_fragment.height(), proto_fragment.height()); @@ -388,10 +388,10 @@ TEST(Proto2RosTesting, MessagesWithAnyFields) { proto_request.mutable_msg()->PackFrom(proto_matrix); auto ros_request = proto2ros_tests::msg::RTTIQueryRequest(); - convert(proto_request, &ros_request); + Convert(proto_request, &ros_request); auto other_proto_request = proto2ros_tests::RTTIQueryRequest(); - convert(ros_request, &other_proto_request); + Convert(ros_request, &other_proto_request); auto other_unpacked_proto_matrix = proto2ros_tests::Matrix(); EXPECT_TRUE(other_proto_request.msg().UnpackTo(&other_unpacked_proto_matrix)); @@ -407,14 +407,14 @@ TEST(Proto2RosTesting, MessagesWithUnknownTypeFields) { proto_result.mutable_type()->set_name("SomeMessage"); auto ros_result = proto2ros_tests::msg::RTTIQueryResult(); - convert(proto_result, &ros_result); + Convert(proto_result, &ros_result); auto unpacked_proto_type = google::protobuf::Type(); - convert(ros_result.type, &unpacked_proto_type); + Convert(ros_result.type, &unpacked_proto_type); EXPECT_EQ(unpacked_proto_type.name(), proto_result.type().name()); auto other_proto_result = proto2ros_tests::RTTIQueryResult(); - convert(ros_result, &other_proto_result); + Convert(ros_result, &other_proto_result); EXPECT_EQ(other_proto_result.type().name(), proto_result.type().name()); } @@ -425,8 +425,8 @@ TEST(Proto2RosTesting, MessagesWithExpandedAnyFields) { proto_target.set_angle(M_PI); auto proto_roi = bosdyn::api::Polygon(); - for (double dx : {-0.5, 0.5}) { - for (double dy : {-0.5, 0.5}) { + for (const double dx : {-0.5, 0.5}) { + for (const double dy : {-0.5, 0.5}) { auto* vertex = proto_roi.add_vertexes(); vertex->set_x(proto_target.position().x() + dx); vertex->set_y(proto_target.position().y() + dy); @@ -438,14 +438,14 @@ TEST(Proto2RosTesting, MessagesWithExpandedAnyFields) { proto_goal.mutable_roi()->PackFrom(proto_roi); auto ros_goal = proto2ros_tests::msg::Goal(); - convert(proto_goal, &ros_goal); + Convert(proto_goal, &ros_goal); EXPECT_EQ(ros_goal.target.position.x, proto_target.position().x()); EXPECT_EQ(ros_goal.target.position.y, proto_target.position().y()); EXPECT_EQ(ros_goal.roi.type_name, "geometry_msgs/Polygon"); auto other_proto_goal = proto2ros_tests::Goal(); - convert(ros_goal, &other_proto_goal); + Convert(ros_goal, &other_proto_goal); auto other_proto_target = bosdyn::api::SE2Pose(); EXPECT_TRUE(other_proto_goal.target().UnpackTo(&other_proto_target)); @@ -462,3 +462,4 @@ TEST(Proto2RosTesting, MessagesWithExpandedAnyFields) { EXPECT_EQ(other_proto_roi.vertexes(i).y(), proto_roi.vertexes(i).y()); } } +// NOLINTEND(readability-magic-numbers,readability-function-cognitive-complexity,cppcoreguidelines-pro-type-reinterpret-cast)