diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..106e7db --- /dev/null +++ b/.clang-format @@ -0,0 +1,15 @@ +# -*- yaml -*- + +# This file determines clang-format's style settings; for details, refer to +# http://clang.llvm.org/docs/ClangFormatStyleOptions.html + +# See https://google.github.io/styleguide/cppguide.html for more info +BasedOnStyle: Google + +Language: Cpp + +SortIncludes: false + +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html#columnlimit +# Maximum line with is 120 characters (default: 80) +ColumnLimit : 120 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6ca6ce6..5d79c8a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -28,6 +28,16 @@ repos: - id: check-merge-conflict - id: check-executables-have-shebangs - id: check-shebang-scripts-are-executable +- repo: https://github.com/ssciwr/clang-format-hook.git + rev: v14.0.0 + hooks: + - id: clang-format + types_or: [c++, c] +- repo: https://github.com/cpplint/cpplint.git + rev: 1.6.1 + hooks: + - id: cpplint + args: ['--quiet', '--filter=-whitespace/comments', '--linelength=120'] - repo: https://github.com/pre-commit/mirrors-mypy rev: v1.2.0 hooks: diff --git a/proto2ros/CMakeLists.txt b/proto2ros/CMakeLists.txt index aab7701..98079d2 100644 --- a/proto2ros/CMakeLists.txt +++ b/proto2ros/CMakeLists.txt @@ -1,5 +1,5 @@ # Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.9) if(POLICY CMP0148) cmake_policy(SET CMP0148 OLD) # to accommodate rosidl pipeline endif() @@ -9,16 +9,69 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +find_program(CCACHE_PROGRAM ccache) +if(CCACHE_PROGRAM) + set(CMAKE_CXX_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") +endif() + +set(DEFAULT_BUILD_TYPE "Release") +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") + set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of the build." FORCE) + # Set the possible values of the build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + +if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") + include(CheckIPOSupported) + check_ipo_supported(RESULT ipo_supported OUTPUT output) + if(ipo_supported) + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) + message(STATUS "We are in release - Successfully enabled IPO") + else() + message(WARNING "IPO not supported - Skipping reason: ${output}") + endif() +endif() + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) + +find_package(Protobuf REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/Any.msg msg/AnyProto.msg msg/Bytes.msg msg/List.msg msg/Struct.msg msg/StructEntry.msg msg/Value.msg ) +add_library(${PROJECT_NAME}_conversions SHARED src/conversions.cpp) +set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON) +target_compile_options(${PROJECT_NAME}_conversions PUBLIC "$<$:-O0>") +target_compile_definitions(${PROJECT_NAME}_conversions + PUBLIC + "$<$:DEBUG>" + "$<$,$>:NDEBUG>" +) +target_include_directories(${PROJECT_NAME}_conversions PUBLIC + "$" + "$" +) +rosidl_get_typesupport_target(${PROJECT_NAME}_cpp_msgs ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_conversions ${${PROJECT_NAME}_cpp_msgs} protobuf::libprotobuf) +ament_target_dependencies(${PROJECT_NAME}_conversions builtin_interfaces rclcpp std_msgs) + +find_program(CLANG_TIDY_EXECUTABLE NAMES "clang-tidy" REQUIRED) +set(CXX_CLANG_TIDY "${CLANG_TIDY_EXECUTABLE}" + "-header-filter=.*proto2ros/.*conversions.hpp" + "-checks=-clang-diagnostic-ignored-optimization-argument") +set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_CLANG_TIDY "${CXX_CLANG_TIDY}") + include(cmake/rosidl_helpers.cmake) rosidl_generated_python_package_add( ${PROJECT_NAME}_additional_modules @@ -26,6 +79,20 @@ rosidl_generated_python_package_add( DESTINATION ${PROJECT_NAME} ) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS ${PROJECT_NAME}_conversions + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +ament_export_targets(${PROJECT_NAME}) + install( DIRECTORY cmake DESTINATION share/${PROJECT_NAME} diff --git a/proto2ros/cmake/proto2ros_generate.cmake b/proto2ros/cmake/proto2ros_generate.cmake index c679566..548f1ef 100644 --- a/proto2ros/cmake/proto2ros_generate.cmake +++ b/proto2ros/cmake/proto2ros_generate.cmake @@ -24,7 +24,7 @@ function(proto2ros_generate target) cmake_parse_arguments( ARG "NO_LINT" - "PACKAGE_NAME;CONFIG_FILE;INTERFACES_OUT_VAR;PYTHON_OUT_VAR" + "PACKAGE_NAME;CONFIG_FILE;INTERFACES_OUT_VAR;PYTHON_OUT_VAR;CPP_OUT_VAR;INCLUDE_OUT_VAR" "PROTOS;PROTO_DESCRIPTORS;IMPORT_DIRS;CONFIG_OVERLAYS;APPEND_PYTHONPATH" ${ARGN}) if(NOT ARG_PACKAGE_NAME) set(ARG_PACKAGE_NAME ${PROJECT_NAME}) @@ -33,7 +33,13 @@ function(proto2ros_generate target) set(ARG_INTERFACES_OUT_VAR ${target}_interfaces) endif() if(NOT ARG_PYTHON_OUT_VAR) - set(ARG_INTERFACES_OUT_VAR ${target}_python_sources) + set(ARG_PYTHON_OUT_VAR ${target}_python_sources) + endif() + if(NOT ARG_CPP_OUT_VAR) + set(ARG_CPP_OUT_VAR ${target}_cpp_sources) + endif() + if(NOT ARG_INCLUDE_OUT_VAR) + set(ARG_INCLUDE_OUT_VAR ${target}_cpp_include) endif() list(APPEND ARG_APPEND_PYTHONPATH "${PROJECT_SOURCE_DIR}") string(REPLACE ";" ":" APPEND_PYTHONPATH "${ARG_APPEND_PYTHONPATH}") @@ -142,6 +148,8 @@ function(proto2ros_generate target) string(REPLACE "${OUTPUT_PATH}/" "${OUTPUT_PATH}:" interface_tuples "${interface_files}") set(python_sources ${output_files}) list(FILTER python_sources INCLUDE REGEX ".*\.py$") + set(cpp_sources ${output_files}) + list(FILTER cpp_sources INCLUDE REGEX ".*\.cpp$") if(BUILD_TESTING AND NOT ARG_NO_LINT AND ament_cmake_mypy_FOUND) set(MYPY_PATH "${APPEND_PYTHONPATH}:$ENV{PYTHONPATH}") @@ -156,4 +164,6 @@ function(proto2ros_generate target) endif() set(${ARG_INTERFACES_OUT_VAR} ${interface_tuples} PARENT_SCOPE) set(${ARG_PYTHON_OUT_VAR} ${python_sources} PARENT_SCOPE) + set(${ARG_CPP_OUT_VAR} ${cpp_sources} PARENT_SCOPE) + set(${ARG_INCLUDE_OUT_VAR} ${BASE_PATH} PARENT_SCOPE) endfunction() diff --git a/proto2ros/cmake/proto2ros_vendor_package.cmake b/proto2ros/cmake/proto2ros_vendor_package.cmake index 121e35c..5c27144 100644 --- a/proto2ros/cmake/proto2ros_vendor_package.cmake +++ b/proto2ros/cmake/proto2ros_vendor_package.cmake @@ -35,6 +35,7 @@ macro(proto2ros_vendor_package target) CONFIG_OVERLAYS ${ARG_CONFIG_OVERLAYS} INTERFACES_OUT_VAR ros_messages PYTHON_OUT_VAR py_sources + CPP_OUT_VAR cpp_sources ${proto2ros_generate_OPTIONS} ) @@ -55,4 +56,28 @@ macro(proto2ros_vendor_package target) PACKAGES ${ARG_PYTHON_PACKAGES} DESTINATION ${target} ) + + find_package(rclcpp REQUIRED) + add_library(${target}_conversions ${cpp_sources}) + rosidl_get_typesupport_target(cpp_interfaces ${target} "rosidl_typesupport_cpp") + target_link_libraries(${target}_conversions ${cpp_interfaces}) + ament_target_dependencies(${target}_conversions + ${ARG_ROS_DEPENDENCIES} builtin_interfaces proto2ros rclcpp) + + set(header_files ${cpp_sources}) + list(FILTER header_files INCLUDE REGEX ".*\.hpp$") + install( + FILES ${header_files} + DESTINATION include/${PROJECT_NAME} + ) + ament_export_include_directories("include/${PROJECT_NAME}") + + install( + TARGETS ${target}_conversions + EXPORT export_${target}_conversions + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + ament_export_targets(export_${target}_conversions) endmacro() diff --git a/proto2ros/include/proto2ros/conversions.hpp b/proto2ros/include/proto2ros/conversions.hpp new file mode 100644 index 0000000..c9a1fcf --- /dev/null +++ b/proto2ros/include/proto2ros/conversions.hpp @@ -0,0 +1,153 @@ +// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +/// This module provides basic conversion APIs, applicable to any proto2ros generated packages. + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace proto2ros { +namespace 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) { + proto_msg->Clear(); + auto wrapper = google::protobuf::Any(); + wrapper.set_type_url(ros_msg.type_url); + auto* value = wrapper.mutable_value(); + value->reserve(ros_msg.value.size()); + value->assign(ros_msg.value.begin(), ros_msg.value.end()); + if (!wrapper.UnpackTo(proto_msg)) { + throw std::runtime_error("failed to unpack AnyProto message"); + } +} + +/// Packs any Protobuf message into a proto2ros/AnyProto ROS message. +template +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(); + const auto& value = wrapper.value(); + ros_msg->value.reserve(value.size()); + ros_msg->value.assign(value.begin(), value.end()); +} + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +} // namespace conversions +} // namespace proto2ros diff --git a/proto2ros/msg/Value.msg b/proto2ros/msg/Value.msg index eefd497..92462c4 100644 --- a/proto2ros/msg/Value.msg +++ b/proto2ros/msg/Value.msg @@ -4,11 +4,12 @@ # See https://protobuf.dev/reference/protobuf/google.protobuf/#value for further reference. int8 NO_VALUE_SET=0 -int8 NUMBER_VALUE_SET=1 -int8 STRING_VALUE_SET=2 -int8 BOOL_VALUE_SET=3 -int8 STRUCT_VALUE_SET=4 -int8 LIST_VALUE_SET=5 +int8 NULL_VALUE_SET=1 +int8 NUMBER_VALUE_SET=2 +int8 STRING_VALUE_SET=3 +int8 BOOL_VALUE_SET=4 +int8 STRUCT_VALUE_SET=5 +int8 LIST_VALUE_SET=6 int8 kind 0 diff --git a/proto2ros/package.xml b/proto2ros/package.xml index 74d9776..926dd32 100644 --- a/proto2ros/package.xml +++ b/proto2ros/package.xml @@ -16,13 +16,19 @@ Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. rosidl_default_generators ament_cmake_mypy - protobuf - protobuf-dev + clang-tidy + + protobuf + protobuf-dev + rclcpp + + builtin_interfaces + std_msgs + + rclpy rosidl_adapter - builtin_interfaces rosidl_default_runtime - std_msgs python3-inflection python3-jinja2 diff --git a/proto2ros/proto2ros-extras.cmake b/proto2ros/proto2ros-extras.cmake index 7351176..5542598 100644 --- a/proto2ros/proto2ros-extras.cmake +++ b/proto2ros/proto2ros-extras.cmake @@ -12,6 +12,7 @@ if(BUILD_TESTING) endif() include("${proto2ros_DIR}/proto2ros_generate.cmake") +find_package(rclcpp REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) include("${proto2ros_DIR}/proto2ros_vendor_package.cmake") diff --git a/proto2ros/proto2ros/cli/generate.py b/proto2ros/proto2ros/cli/generate.py index 99b6545..747de74 100644 --- a/proto2ros/proto2ros/cli/generate.py +++ b/proto2ros/proto2ros/cli/generate.py @@ -15,6 +15,7 @@ from proto2ros.dependencies import fix_dependency_cycles from proto2ros.descriptors.sources import read_source_descriptors from proto2ros.equivalences import Equivalence, all_message_specifications, extract_equivalences +from proto2ros.output.cpp import dump_conversions_cpp_sources, to_pb2_cpp_header, to_ros_cpp_header from proto2ros.output.interfaces import dump_message_specification, which_message_specification from proto2ros.output.python import ( dump_conversions_python_module, @@ -40,8 +41,8 @@ def do_generate(args: argparse.Namespace) -> int: source_paths = [source_descriptor.name, *source_descriptor.dependency] # Add corresponding *_pb2 Python modules to configured Python imports. config.python_imports.update(map(to_pb2_python_module_name, source_paths)) - # Add target ROS package to configured Python imports. - config.python_imports.add(to_ros_python_module_name(args.package_name)) + # Add corresponding *.pb.h C++ headers to configured C++ headers. + config.cpp_headers.update(map(to_pb2_cpp_header, source_paths)) source_descriptors.append(source_descriptor) # Apply overlays to configuration. @@ -57,6 +58,13 @@ def do_generate(args: argparse.Namespace) -> int: message_specifications = list(all_message_specifications(equivalences)) fix_dependency_cycles(message_specifications, quiet=args.dry) + if not config.skip_implicit_imports: + # Add target ROS package to configured Python imports. + config.python_imports.add(to_ros_python_module_name(args.package_name)) + # Add target ROS package to configured C++ headers. + for spec in message_specifications: + config.cpp_headers.add(to_ros_cpp_header(spec)) + # Collect all known message specifications. known_message_specifications = list(message_specifications) for module_name in config.package_specifications: @@ -114,6 +122,21 @@ def do_generate(args: argparse.Namespace) -> int: ) files_written.append(conversions_python_file) + # Write C++ conversion APIs source files. + conversions_hpp_file = args.output_directory / "conversions.hpp" + conversions_cpp_file = args.output_directory / "conversions.cpp" + if not args.dry: + hpp_content, cpp_content = dump_conversions_cpp_sources( + args.package_name, + message_specifications, + known_message_specifications, + config, + ) + conversions_hpp_file.write_text(hpp_content + "\n") + conversions_cpp_file.write_text(cpp_content + "\n") + files_written.append(conversions_hpp_file) + files_written.append(conversions_cpp_file) + if args.manifest_file: # Write generation manifest file. args.manifest_file.write_text("\n".join(map(str, files_written)) + "\n") diff --git a/proto2ros/proto2ros/configuration/__init__.py b/proto2ros/proto2ros/configuration/__init__.py index 7bf6518..39cabab 100644 --- a/proto2ros/proto2ros/configuration/__init__.py +++ b/proto2ros/proto2ros/configuration/__init__.py @@ -59,8 +59,12 @@ class Configuration: package_specifications: Set[str] = dataclasses.field(default_factory=set) + cpp_headers: Set[str] = dataclasses.field(default_factory=set) + inline_cpp_namespaces: Set[str] = dataclasses.field(default_factory=set) + python_imports: Set[str] = dataclasses.field(default_factory=set) inline_python_imports: Set[str] = dataclasses.field(default_factory=set) + skip_implicit_imports: bool = False def __post_init__(self) -> None: @@ -68,7 +72,10 @@ def __post_init__(self) -> None: self.any_expansions = { key: set(value) if not isinstance(value, str) else value for key, value in self.any_expansions.items() } + self.cpp_headers = set(self.cpp_headers) + self.inline_cpp_namespaces = set(self.inline_cpp_namespaces) self.python_imports = set(self.python_imports) + self.inline_python_imports = set(self.inline_python_imports) self.package_specifications = set(self.package_specifications) def update(self, **attributes: Any) -> None: diff --git a/proto2ros/proto2ros/equivalences.py b/proto2ros/proto2ros/equivalences.py index 5cf0cc6..9d8fe02 100644 --- a/proto2ros/proto2ros/equivalences.py +++ b/proto2ros/proto2ros/equivalences.py @@ -435,6 +435,8 @@ def compute_equivalence_for_message( message_spec.annotations["map-entry"] = descriptor.options.map_entry if descriptor.options.map_entry: message_spec.annotations["map-inplace"] = map_inplace + for spec in auxiliary_message_specs: + spec.annotations["parent-spec"] = message_spec return Equivalence( proto_spec=descriptor, message_spec=message_spec, diff --git a/proto2ros/proto2ros/output/cpp.py b/proto2ros/proto2ros/output/cpp.py new file mode 100644 index 0000000..c2b006d --- /dev/null +++ b/proto2ros/proto2ros/output/cpp.py @@ -0,0 +1,109 @@ +# Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +"""This module provides APIs to write C++ conversion code.""" + +import os +from typing import List, Optional, Tuple, Union + +import inflection +import jinja2 +from rosidl_adapter.parser import BaseType, MessageSpecification + +from proto2ros.configuration import Configuration +from proto2ros.utilities import rreplace, to_ros_base_type + + +def to_pb2_cpp_header(source_path: os.PathLike) -> str: + """Returns the C++ header for a given Protobuf source file.""" + basename, _ = os.path.splitext(source_path) + return basename + ".pb.h" + + +def itemize_cpp_identifier(identifier: str, prefix: Optional[str] = None) -> str: + """Derives a loop variable identifier for its iterable variable identifier.""" + basename = identifier.rpartition(".")[-1] + basename = basename.rpartition("->")[-1] + basename = basename.partition("(")[0] + if prefix and not basename.startswith(prefix): + basename = prefix + basename + if not basename.endswith("item"): + return f"{basename}_item" + return basename.removesuffix("item") + "subitem" + + +def to_ros_cpp_header(spec: MessageSpecification) -> str: + """Returns the C++ header for a given ROS message specification.""" + basename = inflection.underscore(spec.base_type.type) + return f"{spec.base_type.pkg_name}/msg/{basename}.hpp" + + +def to_ros_cpp_namespace(package_name: str) -> str: + """Returns the C++ namespace for a given ROS message package.""" + return f"{package_name}::msg" + + +def to_ros_cpp_type(type_name: Union[str, BaseType]) -> str: + """Returns the C++ class name for a given ROS message type.""" + package_name, _, name = str(type_name).rpartition("/") + if not package_name: + raise ValueError(f"no package name in {type_name}") + if not name: + raise ValueError(f"no message name in {type_name}") + return f"{to_ros_cpp_namespace(package_name)}::{name}" + + +def to_pb2_cpp_type(type_name: str) -> str: + """Returns the C++ class name for a given Protobuf message type.""" + return type_name.replace(".", "::") + + +def to_hungarian_notation(name: str) -> str: + """Transforms the given valid C++ name to hungarian notation. + + E.g. some_name_of_mine translates to kSomeNameOfMine. + """ + return "k" + inflection.camelize(name) + + +def dump_conversions_cpp_sources( + package_name: str, + message_specifications: List[MessageSpecification], + known_message_specifications: List[MessageSpecification], + config: Configuration, +) -> Tuple[str, str]: + """Dumps the C++ sources for Protobuf <-> ROS conversion APIs. + + Args: + package_name: name of the package that will host the APIs. + message_specifications: annotated ROS message specifications, + as derived from equivalence relations (see `proto2ros.equivalences`). + known_message_specifications: all annotated ROS message specifications known, + including those from dependencies. A superset of ``message_specifications``. + config: a suitable configuration for the procedure. + + Returns: + the conversion C++ header and source files' content, in that order. + """ + env = jinja2.Environment(loader=jinja2.PackageLoader("proto2ros.output")) + env.globals["itemize_cpp_identifier"] = itemize_cpp_identifier + env.globals["to_ros_base_type"] = to_ros_base_type + env.globals["rreplace"] = rreplace + env.filters["as_ros_base_type"] = to_ros_base_type + env.filters["as_ros_cpp_type"] = to_ros_cpp_type + env.filters["as_pb2_cpp_type"] = to_pb2_cpp_type + env.filters["to_hungarian_notation"] = to_hungarian_notation + env.filters["rreplace"] = rreplace + cpp_conversions_header_template = env.get_template("conversions.hpp.jinja") + cpp_conversions_header_content = cpp_conversions_header_template.render( + package_name=package_name, + message_specifications=message_specifications, + config=config, + ) + cpp_conversions_source_template = env.get_template("conversions.cpp.jinja") + cpp_conversions_source_content = cpp_conversions_source_template.render( + package_name=package_name, + message_specifications=message_specifications, + known_message_specifications={str(spec.base_type): spec for spec in known_message_specifications}, + config=config, + ) + return cpp_conversions_header_content, cpp_conversions_source_content diff --git a/proto2ros/proto2ros/output/templates/conversions.cpp.jinja b/proto2ros/proto2ros/output/templates/conversions.cpp.jinja new file mode 100644 index 0000000..e9afc4b --- /dev/null +++ b/proto2ros/proto2ros/output/templates/conversions.cpp.jinja @@ -0,0 +1,401 @@ +// THIS IS AN AUTOGENERATED FILE. DO NOT EDIT THIS FILE DIRECTLY. +// See conversions.cpp.jinja template in the proto2ros.output.templates Python module. + +#include "{{ package_name }}/conversions.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +{#- + Expands to code for ROS to Protobuf field conversion. + + Args: + source: identifier of the ROS message field to convert from. + destination: identifier of the Protobuf message field to convert to. + spec: annotated ROS message field specification. +-#} +{%- macro ros_to_proto_field_code(source, destination, spec) -%} + {%- if not spec.type.is_primitive_type() -%} + {%- set type_spec = known_message_specifications.get(to_ros_base_type(spec.type)) -%} + {%- 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()); +for (const auto& item : {{ source }}) { + {{ destination }}->Add(static_cast<{{ type_spec.annotations["proto-type"] | as_pb2_cpp_type }}>(item.value)); +} + {%- else -%} +{{ destination | rreplace("mutable", "set", 1) | rreplace("()", "", 1) }}( + static_cast<{{ type_spec.annotations["proto-type"] | as_pb2_cpp_type }}>({{ source }}.value)); + {%- endif -%} + {%- elif spec.type.is_array -%} + {%- set input_item = itemize_cpp_identifier(source, "input_") -%} + {%- set output_item = itemize_cpp_identifier(destination, "output_") -%} + {%- if type_spec and type_spec.annotations.get("map-entry") -%}{#- Handle map case. -#} +{{ destination }}->clear(); +for (const auto& {{ input_item }} : {{ source }}) { + auto* {{ output_item }} = &(*{{ destination }})[{{ input_item }}.key]; + {% for field_spec in type_spec.fields if field_spec.name == "value" %} + {%- if not field_spec.type.is_primitive_type() -%} + {%- set field_type_spec = known_message_specifications.get(to_ros_base_type(spec.type)) -%} + {%- if field_type_spec and field_type_spec.annotations.get("proto-class") == "enum" -%} + *{{ output_item }} = static_cast<{{ field_type_spec.annotations["proto-type"] | as_pb2_cpp_type }}>({{ input_item }}.value); + {%- else -%}{#- Handle message case. -#} + {{ ros_to_proto_composite_field_code_block(input_item + ".value", output_item, field_spec) | indent(4) }} + {%- endif -%} + {%- elif field_spec.annotations["proto-type"] == "bytes" -%} + {#- Handle bytes case. -#} + {{ output_item }}->clear(); + {{ output_item }}->reserve({{ input_item }}.value.size()); + {{ output_item }}->assign({{ input_item }}.value.begin(), {{ input_item }}.value.end()); + {%- else -%}{#- Handle primitive types case. -#} + *{{ output_item }} = {{ input_item }}.value; + {%- endif -%} + {%- endfor %} +} + {%- else -%}{#- Handle sequence case i.e. just convert sequence items. -#} +{{ destination }}->Clear(); +{{ destination }}->Reserve({{ 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) }} +} + {%- endif -%} + {%- else -%}{#- Handle message case. -#} +{{ ros_to_proto_composite_field_code_block(source, destination, spec) }} + {%- endif -%} + {%- elif spec.annotations["proto-type"] == "bytes" -%} + {#- Handle bytes case. -#} + {%- if to_ros_base_type(spec.type) == "proto2ros/Bytes" -%} +{{ destination }}->Clear(); +{{ destination }}->Reserve({{ source }}.size()); +for (const auto& blob : {{ source }}) { + auto* value = {{ destination }}->Add(); + value->reserve(blob.data.size()); + value->assign(blob.data.begin(), blob.data.end()); +} + {%- else -%} +{{ destination }}->clear(); +{{ destination }}->reserve({{ source }}.size()); +{{ destination }}->assign({{ source }}.begin(), {{ source }}.end()); + {%- endif -%} + {%- else -%}{#- Handle primitive types case. -#} + {%- if spec.type.is_array -%} +{{ destination }}->Clear(); +{{ destination }}->Reserve({{ source }}.size()); +for (auto item : {{ source }}) { + {{ destination }}->Add(std::move(item)); +} + {%- else -%} +{{ destination | rreplace("mutable", "set", 1) | rreplace("()", "", 1) }}({{ source }}); + {%- endif -%} + {%- endif -%} +{%- endmacro -%} + +{#- + Expands to a code block for ROS to Protobuf composite field conversion. + + Args: + source: identifier for the ROS message field to convert from. + destination: identifier for the Protobuf message field to convert to. + spec: annotated ROS message field specification. + + Note: code block may expand within a for-loop, during a repeated field expansion. +-#} +{%- macro ros_to_proto_composite_field_code_block(source, destination, spec) -%} + {%- set type_spec = known_message_specifications.get(to_ros_base_type(spec.type)) -%} + {%- if spec.annotations.get("type-erased") -%}{#- ROS message must be deserialized for conversion. -#} +{ + if ({{ source }}.type_name != "{{ spec.type | as_ros_base_type }}") { + std::ostringstream message{"expected {{ spec.type | as_ros_base_type }} message for {{ spec.name }} member"}; + message << ", got " << {{ source }}.type_name; + throw std::runtime_error(message.str()); + } + rclcpp::SerializedMessage serialized_message; + serialized_message.reserve({{ source }}.value.size()); + auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); + std::copy({{ source }}.value.begin(), {{ source }}.value.end(), rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = {{ source }}.value.size(); + 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 }}); +} + {%- 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); + {{ destination }}->PackFrom(typed_proto_message); +} + {%- elif spec.annotations.get("type-casts") -%} + {#- ROS message must be deserialized according to type, then converted, then packed for assignment. -#} + {% for proto_type_name, ros_type_name in spec.annotations["type-casts"] -%} + {%- if loop.first -%} +if ({{ source }}.type_name == "{{ ros_type_name }}") { + {%- else %} +} else if ({{ source }}.type_name == "{{ ros_type_name }}") { + {%- endif %} + rclcpp::SerializedMessage serialized_message; + serialized_message.reserve({{ source }}.value.size()); + auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); + std::copy({{ source }}.value.begin(), {{ source }}.value.end(), rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = {{ source }}.value.size(); + const auto serializer = rclcpp::Serialization<{{ ros_type_name | as_ros_cpp_type }}>{}; + 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); + {{ destination }}->PackFrom(typed_proto_message); + {%- endfor %} +} else { + std::ostringstream message; + message << "unexpected " << {{ source }}.type_name << " in {{ spec.name }} member"; + throw std::runtime_error(message.str()); +} + {%- 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"] }}) { + {%- 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) + "()" %} + case {{ type_spec.base_type | string | as_ros_cpp_type }}::{{ tag_spec.name }}: + {{ ros_to_proto_field_code(source_member, destination_member, member_spec) | indent(8) }} + break; + {%- endfor %} + default: + break; +} + {%- else -%}{#- Handle the generic ROS message case (because it is appropriate or because we do not know any better). -#} +convert({{ source }}, {{ destination }}); + {%- endif -%} +{%- endmacro -%} + +{#- + Expands to code for Protobuf to ROS field conversion. + + Args: + source: identifier of the Protobuf message field to convert from. + destination: identifier of the ROS message field to convert to. + spec: annotated ROS message field specification. +-#} +{%- macro proto_to_ros_field_code(source, destination, spec) -%} + {%- if not spec.type.is_primitive_type() -%} + {%- set type_spec = known_message_specifications.get(to_ros_base_type(spec.type)) %} + {%- if type_spec and type_spec.annotations.get("proto-class") == "enum" -%} + {#- Handle enum case i.e. wrap integral values in ROS messages. -#} + {%- if spec.type.is_array -%} +{{ destination }}.clear(); +{{ destination }}.reserve({{ source }}.size()); +for (const auto& value : {{ source }}) { + auto& mutable_enum_message = {{ destination }}.emplace_back(); + mutable_enum_message.value = static_cast(value); +} + {%- else -%} +{{ destination }}.value = static_cast({{ source }}); + {%- endif -%} + {%- elif spec.type.is_array -%} + {%- set input_item = itemize_cpp_identifier(source, "input_") -%} + {%- set output_item = itemize_cpp_identifier(destination, "output_") -%} + {%- if type_spec and type_spec.annotations.get("map-entry") -%} + {#- Handle map case i.e. reconstruct map entries, the convert, then assign. -#} +for (const auto& {{ input_item }} : {{ source }}) { + auto& {{ output_item }} = {{ destination }}.emplace_back(); + {{ output_item }}.key = {{ input_item }}.first; + {%- for field_spec in type_spec.fields if field_spec.name == "value" %} + {{ proto_to_ros_field_code(input_item + ".second", output_item + ".value", field_spec) | indent(4) }} + {%- endfor %} +} + {%- else -%} + {#- Handle sequence case. -#} +for (const auto& {{ input_item }} : {{ source }}) { + auto &{{ output_item }} = {{ destination }}.emplace_back(); + {{ proto_to_ros_composite_field_code_block(input_item, output_item, spec) | indent(4) }} +} + {%- endif %} + {%- else -%}{#- Handle message case. -#} +{{ proto_to_ros_composite_field_code_block(source, destination, spec) }} + {%- endif -%} + {%- elif spec.annotations["proto-type"] == "bytes" %} + {#- Handle bytes case. -#} + {%- if to_ros_base_type(spec.type) == "proto2ros/Bytes" -%} +{{ destination }}.clear(); +{{ destination }}.reserve({{ source }}.size()); +for (const auto& blob : {{ source }}) { + auto& mutable_message = {{ destination }}.emplace_back(); + mutable_message.data.assign(blob.begin(), blob.end()); +} + {%- else -%} +{{ destination }}.clear(); +{{ destination }}.reserve({{ source }}.size()); +{{ destination }}.assign({{ source }}.begin(), {{ source }}.end()); + {%- endif -%} + {%- else -%}{#- Handle primitive types case. -#} + {%- if spec.type.is_array -%} +{{ destination }}.clear(); +{{ destination }}.reserve({{ source }}.size()); +{{ destination }}.assign({{ source }}.begin(), {{ source }}.end()); + {%- else -%} +{{ destination }} = {{ source }}; + {%- endif -%} + {%- endif -%} +{%- endmacro -%} + +{#- + Expands to a code block for Protobuf to ROS composite field conversion. + + Args: + source: identifier of the Protobuf message field to convert from. + destination: identifier of the ROS message field to convert to. + spec: annotated ROS message field specification. + + Note: code block may expand within a for-loop, during a repeated field expansion. +-#} +{%- macro proto_to_ros_composite_field_code_block(source, destination, spec) -%} + {%- set type_spec = known_message_specifications.get(to_ros_base_type(spec.type)) -%} + {%- 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); + 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; + {{ destination }}.value.assign(begin, end); + {{ destination }}.type_name = "{{ spec.type | as_ros_base_type }}"; +} + {%- elif spec.annotations.get("type-casted") -%} + {#- Protobuf message must be unpacked for conversion. -#} +{ + auto typed_message = {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}(); + if (!{{ source }}.UnpackTo(&typed_message)) { + throw std::runtime_error("Failed to unpack protobuf, internal error"); + } + convert(typed_message, &{{ destination }}); +} + {%- elif spec.annotations.get("type-casts") -%} + {#- Protobuf message must be unpacked according to type, then converted, then serialized for assignment. -#} +{ + {%- for proto_type_name, ros_type_name in spec.annotations["type-casts"] -%} + {%- if loop.first %} + if ({{ source }}.Is<{{ proto_type_name | as_pb2_cpp_type }}>()) { + {%- else %} + } else if ({{ source }}.Is<{{ proto_type_name | as_pb2_cpp_type }}>()) { + {%- endif %} + auto typed_protobuf_message = {{ proto_type_name | as_pb2_cpp_type }}(); + if (!{{ source }}.UnpackTo(&typed_protobuf_message)) { + 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); + 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; + {{ destination }}.value.assign(begin, end); + {{ destination }}.type_name = "{{ ros_type_name }}"; + {%- endfor %} + } else { + std::ostringstream message{"unknown protobuf message type in {{ spec.name }} member"}; + message << ": " << {{ source }}.type_url(); + throw std::runtime_error(message.str()); + } +} + {%- elif type_spec and type_spec.annotations.get("tagged") -%} + {#- Handle one-of field case i.e. determine and convert the Protobuf message member that is set. -#} + {%- set source_parent = source.rpartition(".")[0] -%} + {%- set source_case = rreplace(source, "()", "_case()", 1) -%}{#- adds case suffix to the last getter. -#} + {%- set tag_field_spec = type_spec.annotations["tag"] -%} +switch ({{ source_case }}) { + {%- for tag_spec, member_spec in type_spec.annotations["tagged"] %} + {%- set parent_type_spec = type_spec.annotations["parent-spec"] -%} + {%- set source_member = source_parent + "." + member_spec.annotations.get("proto-name", member_spec.name) + "()" -%} + {%- set destination_member = destination + "." + member_spec.name %} + case {{ parent_type_spec.annotations["proto-type"] | as_pb2_cpp_type }}::{{ member_spec.name | to_hungarian_notation }}: + {{ proto_to_ros_field_code(source_member, destination_member, member_spec) | indent(8) }} + {{ destination }}.{{ tag_field_spec.name }} = {{ type_spec.base_type | string | as_ros_cpp_type }}::{{ tag_spec.name }}; + {{ destination }}.{{ tag_field_spec.annotations["alias"] }} = {{ destination }}.{{ tag_field_spec.name }}; + break; + {%- endfor %} + default: + {{ destination }}.{{ tag_field_spec.name }} = 0; + {{ destination }}.{{ tag_field_spec.annotations["alias"] }} = 0; + break; +} + {%- else -%}{#- Handle the generic Protobuf message case (because it is appropriate or because we do not know any better). -#} +convert({{ source }}, &{{ destination }}); + {%- endif -%} +{%- endmacro %} + +namespace {{ package_name }} { +namespace conversions { + +using proto2ros::conversions::convert; +{%- for namespace in config.inline_cpp_namespaces %} +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) { + assert(proto_msg != nullptr); + {%- if spec.fields %} + proto_msg->Clear(); + {%- for field_spec in spec.fields if field_spec.name != "has_field" %} + {%- 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) { + {{ ros_to_proto_field_code(source, destination, field_spec) | indent(8) }} + } + {%- else %} + {{ ros_to_proto_field_code(source, destination, field_spec) | indent(4) }} + {%- endif -%} + {%- endfor -%} + {% else -%}{#- Handle empty message. #} + (void)ros_msg; + (void)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) { + assert(ros_msg != nullptr); + {%- if spec.fields -%} + {%- if spec.annotations["has-optionals"] %} + 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() + "()" -%} + {%- set destination = "ros_msg->" + field_spec.name -%} + {%- if field_spec.annotations["optional"] -%}{#- Check for field presence before use. #} + if (proto_msg.has_{{ field_spec.annotations.get("proto-name", field_spec.name).lower() }}()) { + {{ proto_to_ros_field_code(source, destination, field_spec) | indent(8) }} + ros_msg->has_field |= {{ spec.base_type | string | as_ros_cpp_type }}::{{ field_spec.name | upper }}_FIELD_SET; + } + {%- else %} + {{ proto_to_ros_field_code(source, destination, field_spec) | indent(4) }} + {%- endif -%} + {%- endfor -%} + {% else -%}{#- Handle empty message. #} + (void)proto_msg; + (void)ros_msg; + {%- endif %} +} + +{% endfor -%} +} // namespace conversions +} // namespace {{ package_name }} diff --git a/proto2ros/proto2ros/output/templates/conversions.hpp.jinja b/proto2ros/proto2ros/output/templates/conversions.hpp.jinja new file mode 100644 index 0000000..4072fc0 --- /dev/null +++ b/proto2ros/proto2ros/output/templates/conversions.hpp.jinja @@ -0,0 +1,35 @@ +// THIS IS AN AUTOGENERATED FILE. DO NOT EDIT THIS FILE DIRECTLY. +// See conversions.hpp.jinja template in the proto2ros.output.templates Python module. + +#pragma once + +{%- for header in config.cpp_headers %} +#include <{{ header }}> +{%- endfor %} + +namespace {{ package_name }} { +namespace 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); + +/// 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); + +/// 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) { + auto ros_msg = {{ spec.base_type | string | as_ros_cpp_type }}(); + 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) { + auto proto_msg = {{ spec.annotations["proto-type"] | as_pb2_cpp_type }}(); + convert(ros_msg, &proto_msg); + return proto_msg; +} + +{% endfor -%} +} // namespace conversions +} // namespace {{ package_name }} diff --git a/proto2ros/proto2ros/utilities.py b/proto2ros/proto2ros/utilities.py index 8288d13..e0d6e9e 100644 --- a/proto2ros/proto2ros/utilities.py +++ b/proto2ros/proto2ros/utilities.py @@ -50,6 +50,11 @@ def pairwise(iterable: Iterable[Any]) -> Iterable[Tuple[Any, Any]]: return zip(a, b) +def rreplace(s: str, old: str, new: str, count: int) -> str: + """Replaces a number of string occurrences in reverse.""" + return s[::-1].replace(old[::-1], new[::-1], count)[::-1] + + def to_ros_base_type(type_: Union[str, BaseType]) -> str: """Returns base type name for a given ROS type.""" return BaseType.__str__(Type(str(type_))) diff --git a/proto2ros/src/conversions.cpp b/proto2ros/src/conversions.cpp new file mode 100644 index 0000000..f92a9ed --- /dev/null +++ b/proto2ros/src/conversions.cpp @@ -0,0 +1,267 @@ +// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +#include "proto2ros/conversions.hpp" + +#include + +#include +#include + +namespace proto2ros { +namespace conversions { + +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(); + value->reserve(ros_msg.value.size()); + value->assign(ros_msg.value.begin(), ros_msg.value.end()); +} + +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) { + proto_msg->set_seconds(ros_msg.sec); + proto_msg->set_nanos(ros_msg.nanosec); +} + +void convert(const google::protobuf::Duration& proto_msg, builtin_interfaces::msg::Duration* ros_msg) { + ros_msg->sec = proto_msg.seconds(); + ros_msg->nanosec = proto_msg.nanos(); +} + +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); +} + +void convert(const google::protobuf::Timestamp& proto_msg, builtin_interfaces::msg::Time* ros_msg) { + ros_msg->sec = proto_msg.seconds(); + ros_msg->nanosec = proto_msg.nanos(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + ros_msg->data = proto_msg.value(); +} + +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) { + 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) { + proto_msg->Clear(); + switch (ros_msg.kind) { + case proto2ros::msg::Value::NUMBER_VALUE_SET: + proto_msg->set_number_value(ros_msg.number_value); + break; + case proto2ros::msg::Value::STRING_VALUE_SET: + proto_msg->set_string_value(ros_msg.string_value); + break; + case proto2ros::msg::Value::BOOL_VALUE_SET: + proto_msg->set_bool_value(ros_msg.bool_value); + break; + case proto2ros::msg::Value::STRUCT_VALUE_SET: { + if (ros_msg.struct_value.type_name != "proto2ros/Struct") { + std::ostringstream message{"expected proto2ros/Struct message for struct_value member"}; + message << ", got " << ros_msg.struct_value.type_name; + throw std::runtime_error(message.str()); + } + rclcpp::SerializedMessage serialized_message; + const auto& value = ros_msg.struct_value.value; + serialized_message.reserve(value.size()); + auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); + std::copy(value.begin(), value.end(), rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = value.size(); + 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()); + } break; + case proto2ros::msg::Value::LIST_VALUE_SET: { + if (ros_msg.list_value.type_name != "proto2ros/List") { + std::ostringstream message{"expected proto2ros/Struct message for list_value member"}; + message << ", got " << ros_msg.list_value.type_name; + throw std::runtime_error(message.str()); + } + rclcpp::SerializedMessage serialized_message; + const auto& value = ros_msg.list_value.value; + serialized_message.reserve(value.size()); + auto& rcl_serialized_message = serialized_message.get_rcl_serialized_message(); + std::copy(value.begin(), value.end(), rcl_serialized_message.buffer); + rcl_serialized_message.buffer_length = value.size(); + 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()); + } break; + case proto2ros::msg::Value::NULL_VALUE_SET: + proto_msg->set_null_value(google::protobuf::NullValue::NULL_VALUE); + break; + default: + break; + } +} + +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(); + ros_msg->kind = proto2ros::msg::Value::NUMBER_VALUE_SET; + break; + case google::protobuf::Value::kStringValue: + ros_msg->string_value = proto_msg.string_value(); + ros_msg->kind = proto2ros::msg::Value::STRING_VALUE_SET; + break; + case google::protobuf::Value::kBoolValue: + ros_msg->bool_value = proto_msg.bool_value(); + ros_msg->kind = proto2ros::msg::Value::BOOL_VALUE_SET; + break; + case google::protobuf::Value::kStructValue: { + auto typed_ros_message = proto2ros::msg::Struct(); + 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; + 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); + 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; + 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; + } break; + case google::protobuf::Value::kNullValue: + ros_msg->kind = proto2ros::msg::Value::NULL_VALUE_SET; + break; + default: + ros_msg->kind = proto2ros::msg::Value::NO_VALUE_SET; + break; + } +} + +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()); + for (const auto& input_item : ros_msg.values) { + auto* output_item = values->Add(); + convert(input_item, output_item); + } +} + +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); + } +} + +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]); + } +} + +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); + } +} + +} // namespace conversions +} // namespace proto2ros diff --git a/proto2ros_tests/CMakeLists.txt b/proto2ros_tests/CMakeLists.txt index 568f582..9436ccf 100644 --- a/proto2ros_tests/CMakeLists.txt +++ b/proto2ros_tests/CMakeLists.txt @@ -5,6 +5,25 @@ if(POLICY CMP0148) endif() project(proto2ros_tests) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_program(CCACHE_PROGRAM ccache) +if(CCACHE_PROGRAM) + set(CMAKE_CXX_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") +endif() + +set(DEFAULT_BUILD_TYPE "Release") +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") + set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of the build." FORCE) + # Set the possible values of the build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) @@ -21,6 +40,8 @@ proto2ros_generate( CONFIG_OVERLAYS config/overlay.yaml INTERFACES_OUT_VAR ros_messages PYTHON_OUT_VAR py_sources + CPP_OUT_VAR cpp_sources + INCLUDE_OUT_VAR cpp_include_dir APPEND_PYTHONPATH "${PROTO_OUT_DIR}" ) add_dependencies( @@ -34,6 +55,24 @@ rosidl_generate_interfaces( ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_messages_gen) +add_library(${PROJECT_NAME}_conversions SHARED ${cpp_sources} src/manual_conversions.cpp) +set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON) +target_compile_options(${PROJECT_NAME}_conversions PUBLIC "$<$:-O0>") +target_compile_definitions(${PROJECT_NAME}_conversions + PUBLIC + "$<$:DEBUG>" + "$<$,$>:NDEBUG>" +) +target_include_directories(${PROJECT_NAME}_conversions PUBLIC + "$" + "$" + "$" +) +rosidl_get_typesupport_target(${PROJECT_NAME}_cpp_msgs ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_conversions + ${${PROJECT_NAME}_cpp_msgs} ${PROJECT_NAME}_proto + proto2ros::proto2ros_conversions protobuf::libprotobuf) + rosidl_generated_python_package_add( ${PROJECT_NAME}_additional_modules MODULES ${proto_py_sources} ${py_sources} @@ -41,8 +80,14 @@ rosidl_generated_python_package_add( DESTINATION ${PROJECT_NAME} ) +find_program(CLANG_TIDY_EXECUTABLE NAMES "clang-tidy" REQUIRED) +set(CXX_CLANG_TIDY "${CLANG_TIDY_EXECUTABLE}" "-header-filter=.*proto2ros.*/.*conversions.hpp") +set_target_properties(${PROJECT_NAME}_conversions PROPERTIES CXX_CLANG_TIDY "${CXX_CLANG_TIDY}") + if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + get_rosidl_generated_interfaces_test_setup( LIBRARY_DIRS additional_library_dirs ENV additional_env @@ -50,12 +95,24 @@ if(BUILD_TESTING) list(APPEND additional_env "PYTHONPATH=${PROTO_OUT_DIR}") ament_add_pytest_test( - regression_tests test + regression_py_tests test APPEND_LIBRARY_DIRS ${additional_library_dirs} APPEND_ENV ${additional_env} ENV CMAKE_BINARY_DIR=${CMAKE_BINARY_DIR} PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION=python ) + + ament_add_gtest( + regression_cpp_tests test/test_proto2ros.cpp + APPEND_LIBRARY_DIRS ${additional_library_dirs} + ENV CMAKE_BINARY_DIR=${CMAKE_BINARY_DIR} + ) + target_link_libraries(regression_cpp_tests + ${${PROJECT_NAME}_cpp_msgs} ${PROJECT_NAME}_proto + ${PROJECT_NAME}_conversions proto2ros::proto2ros_conversions + ) + set_target_properties(regression_cpp_tests PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON) + set_target_properties(regression_cpp_tests PROPERTIES CXX_CLANG_TIDY "${CXX_CLANG_TIDY}") endif() ament_package() diff --git a/proto2ros_tests/config/overlay.yaml b/proto2ros_tests/config/overlay.yaml index 0804767..679d389 100644 --- a/proto2ros_tests/config/overlay.yaml +++ b/proto2ros_tests/config/overlay.yaml @@ -14,5 +14,14 @@ message_mapping: python_imports: - bosdyn.api.geometry_pb2 - geometry_msgs.msg +cpp_headers: + - bosdyn/api/geometry.pb.h + - geometry_msgs/msg/polygon.hpp + - geometry_msgs/msg/pose.hpp + - geometry_msgs/msg/quaternion.hpp + - geometry_msgs/msg/twist.hpp + - geometry_msgs/msg/vector3.hpp + - geometry_msgs/msg/wrench.hpp + - proto2ros_tests/manual_conversions.hpp inline_python_imports: - proto2ros_tests.manual_conversions diff --git a/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp b/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp new file mode 100644 index 0000000..28d0f2d --- /dev/null +++ b/proto2ros_tests/include/proto2ros_tests/manual_conversions.hpp @@ -0,0 +1,80 @@ +// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace proto2ros_tests { +namespace 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +/// 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); + +} // namespace conversions +} // namespace proto2ros_tests diff --git a/proto2ros_tests/package.xml b/proto2ros_tests/package.xml index 7f8ae6f..2d2a7d3 100644 --- a/proto2ros_tests/package.xml +++ b/proto2ros_tests/package.xml @@ -13,18 +13,22 @@ Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. ament_cmake rosidl_default_generators - protobuf - proto2ros + clang-tidy - builtin_interfaces - geometry_msgs - sensor_msgs + protobuf + protobuf-dev + proto2ros + + builtin_interfaces + geometry_msgs + sensor_msgs python3-jinja2 python3-networkx python3-protobuf python3-yaml + ament_cmake_gtest ament_cmake_pytest rosidl_interface_packages diff --git a/proto2ros_tests/proto/CMakeLists.txt b/proto2ros_tests/proto/CMakeLists.txt index 554a78e..2b1f899 100644 --- a/proto2ros_tests/proto/CMakeLists.txt +++ b/proto2ros_tests/proto/CMakeLists.txt @@ -10,16 +10,32 @@ protobuf_generate( OUT_VAR bosdyn_proto_py_sources PROTOS bosdyn/api/geometry.proto ) - protobuf_generate( LANGUAGE python OUT_VAR proto_py_sources PROTOS test.proto ) +protobuf_generate( + LANGUAGE cpp + OUT_VAR bosdyn_proto_cpp_sources + PROTOS bosdyn/api/geometry.proto +) +protobuf_generate( + LANGUAGE cpp + OUT_VAR proto_cpp_sources + PROTOS test.proto +) + +add_library(${PROJECT_NAME}_proto SHARED ${proto_cpp_sources} ${bosdyn_proto_cpp_sources}) +target_include_directories(${PROJECT_NAME}_proto PUBLIC "${CMAKE_CURRENT_BINARY_DIR}") +target_compile_options(${PROJECT_NAME}_proto PUBLIC -Wno-deprecated-declarations) +target_link_libraries(${PROJECT_NAME}_proto protobuf::libprotobuf) + add_custom_target( ${PROJECT_NAME}_proto_gen ALL DEPENDS ${proto_py_sources} ${bosdyn_proto_py_sources} + ${proto_cpp_sources} ${bosdyn_proto_cpp_sources} ) set(PROTO_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" PARENT_SCOPE) diff --git a/proto2ros_tests/src/manual_conversions.cpp b/proto2ros_tests/src/manual_conversions.cpp new file mode 100644 index 0000000..da659a4 --- /dev/null +++ b/proto2ros_tests/src/manual_conversions.cpp @@ -0,0 +1,147 @@ +// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +#include "proto2ros_tests/manual_conversions.hpp" + +#include + +namespace proto2ros_tests { +namespace conversions { + +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) { + 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) { + 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) { + 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) { + 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) { + 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 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) { + 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)); +} + +void convert(const bosdyn::api::SE2Pose& proto_msg, geometry_msgs::msg::Pose* ros_msg) { + ros_msg->position.x = proto_msg.position().x(); + ros_msg->position.y = proto_msg.position().y(); + ros_msg->orientation.w = std::cos(proto_msg.angle() / 2.0); + ros_msg->orientation.z = std::sin(proto_msg.angle() / 2.0); +} + +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()); + for (const auto& point : ros_msg.points) { + auto* vertex = vertices->Add(); + vertex->set_x(point.x); + vertex->set_y(point.y); + } +} + +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(); + } +} + +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) { + 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 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 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) { + proto_msg->set_scale(Temperature::CELSIUS); + proto_msg->set_value(ros_msg.temperature); +} + +void convert(const Temperature& proto_msg, sensor_msgs::msg::Temperature* ros_msg) { + switch (proto_msg.scale()) { + case Temperature::KELVIN: + ros_msg->temperature = proto_msg.value() + 273.0; + break; + case Temperature::FAHRENHEIT: + ros_msg->temperature = (proto_msg.value() - 32.0) * 5.0 / 9.0; + break; + case Temperature::CELSIUS: + default: + ros_msg->temperature = proto_msg.value(); + break; + } +} + +} // namespace conversions +} // namespace proto2ros_tests diff --git a/proto2ros_tests/test/test_proto2ros.cpp b/proto2ros_tests/test/test_proto2ros.cpp new file mode 100644 index 0000000..665090d --- /dev/null +++ b/proto2ros_tests/test/test_proto2ros.cpp @@ -0,0 +1,464 @@ +// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using proto2ros::conversions::convert; +using proto2ros_tests::conversions::convert; + +TEST(Proto2RosTesting, MessageMapping) { + auto proto_request = proto2ros_tests::HVACControlRequest(); + proto_request.set_air_flow_rate(1000.0); + auto* setpoint = proto_request.mutable_temperature_setpoint(); + setpoint->set_value(77.0); + setpoint->set_scale(proto2ros_tests::Temperature::FAHRENHEIT); + + auto ros_request = proto2ros_tests::msg::HVACControlRequest(); + convert(proto_request, &ros_request); + + auto other_proto_request = proto2ros_tests::HVACControlRequest(); + 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 + EXPECT_DOUBLE_EQ(other_proto_request.temperature_setpoint().value(), 25.0); + EXPECT_EQ(other_proto_request.temperature_setpoint().scale(), proto2ros_tests::Temperature::CELSIUS); +} + +TEST(Proto2RosTesting, RecursiveMessages) { + auto proto_fragment = proto2ros_tests::Fragment(); + proto_fragment.set_payload("important data"); + auto* proto_subfragment = proto_fragment.add_nested(); + proto_subfragment->set_payload("important addendum"); + + auto ros_fragment = proto2ros_tests::msg::Fragment(); + convert(proto_fragment, &ros_fragment); + + const auto payload = + std::string_view{reinterpret_cast(ros_fragment.payload.data()), ros_fragment.payload.size()}; + EXPECT_EQ(payload, proto_fragment.payload()); + 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); + 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); + EXPECT_EQ(other_proto_subfragment.payload(), proto_subfragment->payload()); +} + +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); + auto* proto_list = (*proto_items)["range"].mutable_list(); + for (double number : {-0.1, 0.0, 0.1, -0.7, 0.3, 0.4}) { + proto_list->add_values()->set_number(number); + } + + auto ros_value = proto2ros_tests::msg::Value(); + convert(proto_value, &ros_value); + + auto other_proto_value = proto2ros_tests::Value(); + convert(ros_value, &other_proto_value); + + const auto& other_proto_items = other_proto_value.dict().items(); + EXPECT_TRUE(other_proto_items.count("interval")); + const auto& other_proto_pair = other_proto_items.at("interval").pair(); + EXPECT_EQ(other_proto_pair.first().number(), proto_pair->first().number()); + EXPECT_EQ(other_proto_pair.second().number(), proto_pair->second().number()); + EXPECT_TRUE(other_proto_items.count("range")); + const auto& other_proto_list = other_proto_items.at("range").list(); + ASSERT_EQ(other_proto_list.values_size(), proto_list->values_size()); + for (int i = 0; i < proto_list->values_size(); ++i) { + EXPECT_EQ(other_proto_list.values(i).number(), proto_list->values(i).number()); + } +} + +TEST(Proto2RosTesting, MessagesWithEnums) { + auto proto_motion_request = proto2ros_tests::MotionRequest(); + 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); + 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); + EXPECT_EQ(other_proto_motion_request.direction(), proto_motion_request.direction()); + EXPECT_EQ(other_proto_motion_request.speed(), proto_motion_request.speed()); +} + +TEST(Proto2RosTesting, MessagesWithNestedEnums) { + auto proto_http_request = proto2ros_tests::HTTP::Request(); + proto_http_request.set_method(proto2ros_tests::HTTP::PUT); + proto_http_request.set_uri("https://proto2ros.xyz/post"); + + auto ros_http_request = proto2ros_tests::msg::HTTPRequest(); + 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); + EXPECT_EQ(other_proto_http_request.method(), proto_http_request.method()); + EXPECT_EQ(other_proto_http_request.uri(), proto_http_request.uri()); +} + +TEST(Proto2RosTesting, OneOfMessages) { + auto proto_any_command = proto2ros_tests::AnyCommand(); + proto_any_command.mutable_walk()->set_distance(1.0); + 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); + 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); + 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()); +} + +TEST(Proto2RosTesting, OneOfEmptyMessages) { + auto proto_any_command = proto2ros_tests::AnyCommand(); + (void)proto_any_command.mutable_sit(); + + auto ros_any_command = proto2ros_tests::msg::AnyCommand(); + 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); + + auto other_proto_any_command = proto2ros_tests::AnyCommand(); + convert(ros_any_command, &other_proto_any_command); + EXPECT_TRUE(other_proto_any_command.has_sit()); +} + +TEST(Proto2RosTesting, MessagesWithMapField) { + auto proto_diagnostic = proto2ros_tests::Diagnostic(); + proto_diagnostic.set_severity(proto2ros_tests::Diagnostic::FATAL); + proto_diagnostic.set_reason("Localization estimate diverged, cannot recover"); + auto& proto_attributes = *proto_diagnostic.mutable_attributes(); + proto_attributes["origin"] = "localization subsystem"; + + auto ros_diagnostic = proto2ros_tests::msg::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()); + EXPECT_EQ(ros_diagnostic.attributes.size(), static_cast(proto_attributes.size())); + EXPECT_EQ(ros_diagnostic.attributes[0].key, "origin"); + EXPECT_EQ(ros_diagnostic.attributes[0].value, "localization subsystem"); + + auto other_proto_diagnostic = proto2ros_tests::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")); + EXPECT_EQ(other_proto_diagnostic.attributes().at("origin"), "localization subsystem"); +} + +TEST(Proto2RosTesting, MessagesWithDeprecatedFields) { + auto proto_remote_execution_request = proto2ros_tests::RemoteExecutionRequest(); + proto_remote_execution_request.set_prompt("grab bike seat"); + proto_remote_execution_request.set_timeout(10000000000); + 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); + 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); + 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()); +} + +TEST(Proto2RosTesting, RedefinedMessages) { + auto proto_remote_execution_result = proto2ros_tests::RemoteExecutionResult(); + proto_remote_execution_result.set_ok(false); + auto* proto_remote_execution_error = proto_remote_execution_result.mutable_error(); + proto_remote_execution_error->set_code(255); + proto_remote_execution_error->set_reason("interrupted"); + proto_remote_execution_error->add_traceback(""); + + auto ros_remote_execution_result = proto2ros_tests::msg::RemoteExecutionResult(); + 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()); + EXPECT_EQ(ros_remote_execution_result.error.traceback.size(), + static_cast(proto_remote_execution_error->traceback_size())); + 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); + 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()); + EXPECT_EQ(other_proto_remote_execution_result.error().traceback_size(), + proto_remote_execution_error->traceback_size()); + EXPECT_EQ(other_proto_remote_execution_result.error().traceback(0), + proto_remote_execution_result.error().traceback(0)); +} + +namespace { + +template +struct has_rotation_member : std::false_type {}; + +// specialization recognizes types that do have a nested ::type member: +template +struct has_rotation_member().rotation)>> : std::true_type {}; + +template +constexpr bool has_rotation_member_v = has_rotation_member::value; + +} // namespace + +TEST(Proto2RosTesting, MessagesWithReservedFields) { + auto proto_displacement = proto2ros_tests::Displacement(); + proto_displacement.mutable_translation()->set_x(1.0); + proto_displacement.mutable_translation()->set_y(2.0); + proto_displacement.mutable_translation()->set_z(3.0); + + auto ros_displacement = proto2ros_tests::msg::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); + 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()); +} + +TEST(Proto2RosTesting, MessageForwarding) { + auto proto_camera_info = proto2ros_tests::CameraInfo(); + proto_camera_info.set_height(720); + 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}) { + 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}) { + 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}) { + 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}) { + proto_camera_info.mutable_distortion_model()->add_coefficients(coeff); + } + + auto ros_camera_info = proto2ros_tests::msg::CameraInfo(); + 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()); + EXPECT_EQ(ros_camera_info.k.cols, proto_camera_info.k().cols()); + EXPECT_EQ(ros_camera_info.k.data.size(), static_cast(proto_camera_info.k().data_size())); + for (size_t i = 0; i < ros_camera_info.k.data.size(); ++i) { + EXPECT_EQ(ros_camera_info.k.data[i], proto_camera_info.k().data(i)); + } + EXPECT_EQ(ros_camera_info.r.rows, proto_camera_info.r().rows()); + EXPECT_EQ(ros_camera_info.r.cols, proto_camera_info.r().cols()); + EXPECT_EQ(ros_camera_info.r.data.size(), static_cast(proto_camera_info.r().data_size())); + for (size_t i = 0; i < ros_camera_info.r.data.size(); ++i) { + EXPECT_EQ(ros_camera_info.r.data[i], proto_camera_info.r().data(i)); + } + EXPECT_EQ(ros_camera_info.p.rows, proto_camera_info.p().rows()); + EXPECT_EQ(ros_camera_info.p.cols, proto_camera_info.p().cols()); + EXPECT_EQ(ros_camera_info.p.data.size(), static_cast(proto_camera_info.p().data_size())); + for (size_t i = 0; i < ros_camera_info.p.data.size(); ++i) { + EXPECT_EQ(ros_camera_info.p.data[i], proto_camera_info.p().data(i)); + } + EXPECT_EQ(ros_camera_info.distortion_model.type.value, proto_camera_info.distortion_model().type()); + EXPECT_EQ(ros_camera_info.distortion_model.coefficients.size(), + static_cast(proto_camera_info.distortion_model().coefficients_size())); + for (size_t i = 0; i < ros_camera_info.distortion_model.coefficients.size(); ++i) { + EXPECT_EQ(ros_camera_info.distortion_model.coefficients[i], proto_camera_info.distortion_model().coefficients(i)); + } + + auto other_proto_camera_info = proto2ros_tests::CameraInfo(); + 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()); + EXPECT_EQ(other_proto_camera_info.k().cols(), proto_camera_info.k().cols()); + EXPECT_EQ(other_proto_camera_info.k().data_size(), proto_camera_info.k().data_size()); + for (int i = 0; i < other_proto_camera_info.k().data_size(); ++i) { + EXPECT_EQ(other_proto_camera_info.k().data(i), proto_camera_info.k().data(i)); + } + EXPECT_EQ(other_proto_camera_info.r().rows(), proto_camera_info.r().rows()); + EXPECT_EQ(other_proto_camera_info.r().cols(), proto_camera_info.r().cols()); + EXPECT_EQ(other_proto_camera_info.r().data_size(), proto_camera_info.r().data_size()); + for (int i = 0; i < other_proto_camera_info.r().data_size(); ++i) { + EXPECT_EQ(other_proto_camera_info.r().data(i), proto_camera_info.r().data(i)); + } + EXPECT_EQ(other_proto_camera_info.p().rows(), proto_camera_info.p().rows()); + EXPECT_EQ(other_proto_camera_info.p().cols(), proto_camera_info.p().cols()); + EXPECT_EQ(other_proto_camera_info.p().data_size(), proto_camera_info.p().data_size()); + for (int i = 0; i < other_proto_camera_info.p().data_size(); ++i) { + EXPECT_EQ(other_proto_camera_info.p().data(i), proto_camera_info.p().data(i)); + } + EXPECT_EQ(other_proto_camera_info.distortion_model().type(), proto_camera_info.distortion_model().type()); + EXPECT_EQ(other_proto_camera_info.distortion_model().coefficients_size(), + proto_camera_info.distortion_model().coefficients_size()); + for (int i = 0; i < other_proto_camera_info.distortion_model().coefficients_size(); ++i) { + EXPECT_EQ(other_proto_camera_info.distortion_model().coefficients(i), + proto_camera_info.distortion_model().coefficients(i)); + } +} + +TEST(Proto2RosTesting, MessagesWithSubMessageMapField) { + auto proto_map = proto2ros_tests::Map(); + auto& proto_submaps = *proto_map.mutable_submaps(); + auto& proto_fragment = proto_submaps[13]; + proto_fragment.set_height(20); + proto_fragment.set_width(20); + auto* proto_grid = proto_fragment.mutable_grid(); + proto_grid->resize(20 * 20, '\0'); + + auto ros_map = proto2ros_tests::msg::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; + EXPECT_EQ(ros_fragment.height, proto_fragment.height()); + EXPECT_EQ(ros_fragment.width, proto_fragment.width()); + EXPECT_EQ(ros_fragment.grid.size(), static_cast(proto_grid->size())); + const auto grid = std::string_view{reinterpret_cast(ros_fragment.grid.data()), ros_fragment.grid.size()}; + EXPECT_EQ(grid, proto_fragment.grid()); + + auto other_proto_map = proto2ros_tests::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()); + EXPECT_EQ(other_proto_fragment.width(), proto_fragment.width()); + EXPECT_EQ(other_proto_fragment.grid(), proto_fragment.grid()); +} + +TEST(Proto2RosTesting, MessagesWithAnyFields) { + auto proto_matrix = proto2ros_tests::Matrix(); + proto_matrix.set_rows(1); + proto_matrix.set_cols(1); + proto_matrix.add_data(0.0); + + auto proto_request = proto2ros_tests::RTTIQueryRequest(); + proto_request.mutable_msg()->PackFrom(proto_matrix); + + auto ros_request = proto2ros_tests::msg::RTTIQueryRequest(); + convert(proto_request, &ros_request); + + auto other_proto_request = proto2ros_tests::RTTIQueryRequest(); + 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)); + + EXPECT_EQ(other_unpacked_proto_matrix.rows(), proto_matrix.rows()); + EXPECT_EQ(other_unpacked_proto_matrix.cols(), proto_matrix.cols()); + EXPECT_EQ(other_unpacked_proto_matrix.data_size(), proto_matrix.data_size()); + EXPECT_EQ(other_unpacked_proto_matrix.data(0), proto_matrix.data(0)); +} + +TEST(Proto2RosTesting, MessagesWithUnknownTypeFields) { + auto proto_result = proto2ros_tests::RTTIQueryResult(); + proto_result.mutable_type()->set_name("SomeMessage"); + + auto ros_result = proto2ros_tests::msg::RTTIQueryResult(); + convert(proto_result, &ros_result); + + auto unpacked_proto_type = google::protobuf::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); + EXPECT_EQ(other_proto_result.type().name(), proto_result.type().name()); +} + +TEST(Proto2RosTesting, MessagesWithExpandedAnyFields) { + auto proto_target = bosdyn::api::SE2Pose(); + proto_target.mutable_position()->set_x(1.0); + proto_target.mutable_position()->set_y(-1.0); + 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}) { + auto* vertex = proto_roi.add_vertexes(); + vertex->set_x(proto_target.position().x() + dx); + vertex->set_y(proto_target.position().y() + dy); + } + } + + auto proto_goal = proto2ros_tests::Goal(); + proto_goal.mutable_target()->PackFrom(proto_target); + proto_goal.mutable_roi()->PackFrom(proto_roi); + + auto ros_goal = proto2ros_tests::msg::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); + + auto other_proto_target = bosdyn::api::SE2Pose(); + EXPECT_TRUE(other_proto_goal.target().UnpackTo(&other_proto_target)); + + EXPECT_EQ(proto_target.position().x(), other_proto_target.position().x()); + EXPECT_EQ(proto_target.position().y(), other_proto_target.position().y()); + EXPECT_EQ(proto_target.angle(), other_proto_target.angle()); + + auto other_proto_roi = bosdyn::api::Polygon(); + EXPECT_TRUE(other_proto_goal.roi().UnpackTo(&other_proto_roi)); + EXPECT_EQ(other_proto_roi.vertexes_size(), proto_roi.vertexes_size()); + for (int i = 0; i < proto_roi.vertexes_size(); ++i) { + EXPECT_EQ(other_proto_roi.vertexes(i).x(), proto_roi.vertexes(i).x()); + EXPECT_EQ(other_proto_roi.vertexes(i).y(), proto_roi.vertexes(i).y()); + } +}