import array import importlib from collections import OrderedDict from typing import TYPE_CHECKING, Any, Dict, Optional, Type from rosidl_parser.definition import NamespacedType # type: ignore from rosidl_runtime_py import ( # type: ignore import_message_from_namespaced_type, message_to_ordereddict, set_message_fields, ) from msgcenterpy.core.envelope import MessageEnvelope, create_envelope from msgcenterpy.core.message_instance import MessageInstance from msgcenterpy.core.type_converter import TypeConverter from msgcenterpy.core.type_info import ConstraintType, Consts, TypeInfo from msgcenterpy.core.types import MessageType if TYPE_CHECKING: from msgcenterpy.core.field_accessor import FieldAccessor class ROS2MessageInstance(MessageInstance[Any]): """ROS2消息实例，支持类型信息提取和字段访问器""" ros_msg_cls: Type[Any] = None # type: ignore @classmethod def get_ros_msg_cls_path(cls, ros_msg_cls: Type[Any]) -> str: return ros_msg_cls.__module__ + "." + ros_msg_cls.__name__ @property def ros_msg_cls_path(self) -> str: return self.get_ros_msg_cls_path(self.ros_msg_cls) @classmethod def get_ros_msg_cls_namespace(cls, ros_msg_cls: Type[Any]) -> str: class_name, module_name = ros_msg_cls.__name__, ros_msg_cls.__module__ package = module_name.split(".")[0] if module_name else "" interface = ( "msg" if ".msg" in module_name else "srv" if ".srv" in module_name else "action" if ".action" in module_name else "msg" ) return f"{package}/{interface}/{class_name}" if package and class_name else f"{module_name}.{class_name}" @property def ros_msg_cls_namespace(self) -> str: return self.get_ros_msg_cls_namespace(self.ros_msg_cls) @classmethod def obtain_ros_cls_from_str(cls, message_type: str | Type[Any]) -> Type[Any]: # 需要先解析出正确的消息类 if isinstance(message_type, str): if "/" in message_type: namespace, name = message_type.rsplit("/", 1) message_type = import_message_from_namespaced_type(NamespacedType(namespace.split("/"), name)) elif "." in message_type: module_path, class_name = message_type.rsplit(".", 1) mod = importlib.import_module(module_path) message_type = getattr(mod, class_name) return message_type # type: ignore def __init__(self, inner_data: Any, **kwargs: Any) -> None: self.ros_msg_cls = inner_data.__class__ if not isinstance(self.ros_msg_cls, type): raise TypeError(f"Expected ROS message class to be a type, got {type(self.ros_msg_cls)}") super().__init__(inner_data, MessageType.ROS2) def export_to_envelope(self, **kwargs: Any) -> MessageEnvelope: """导出为统一信封字典 用户可从 metadata.properties 中读取： - properties.ros_msg_cls_namespace - properties.ros_msg_cls_path """ base_dict = self.get_python_dict() export_envelope = create_envelope( format_name=self.message_type.value, content=base_dict, metadata={ "current_format": self.message_type.value, "source_cls_name": self.inner_data.__class__.__name__, "source_cls_module": self.inner_data.__class__.__module__, **self._metadata, }, ) return export_envelope @classmethod def _ordered_to_dict(cls, obj: Any) -> Any: if isinstance(obj, OrderedDict): return {k: cls._ordered_to_dict(v) for k, v in obj.items()} elif isinstance(obj, tuple): return tuple(cls._ordered_to_dict(v) for v in obj) elif isinstance(obj, (list, array.array)): return [cls._ordered_to_dict(v) for v in obj] else: return obj @classmethod def import_from_envelope(cls, data: MessageEnvelope, **kwargs: Any) -> "ROS2MessageInstance": """从规范信封创建ROS2实例（仅 data 一个参数）。 类型信息从 data.metadata.properties 读取 """ content = data["content"] properties = data["metadata"]["properties"] ros_msg_cls = cls.obtain_ros_cls_from_str(properties["ros_msg_cls_namespace"]) or cls.obtain_ros_cls_from_str( properties["ros_msg_cls_path"] ) if ros_msg_cls is None: raise ValueError( "ros2 type must be provided via metadata.properties.ros_msg_cls_namespace or legacy type_info.ros_namespaced" ) ros_msg = ros_msg_cls() set_message_fields(ros_msg, content) instance = ROS2MessageInstance(ros_msg) return instance def get_python_dict(self) -> Dict[str, Any]: """获取当前所有的字段名和对应的原始值，使用 SLOT_TYPES 进行类型推断和嵌套导入""" base_obj = message_to_ordereddict(self.inner_data) base_dict = self._ordered_to_dict(base_obj) return base_dict # type: ignore[no-any-return] def set_python_dict(self, value: Dict[str, Any], **kwargs: Any) -> bool: """获取当前所有的字段名和对应的原始值，使用 SLOT_TYPES 进行类型推断和嵌套导入""" timestamp_fields = set_message_fields(self.inner_data, value, **kwargs) # todo: 因为ROS自身机制，字段并不会增减，所以需要更新cache中所有accessor的值（通过parent获取） return True # TypeInfoProvider 接口实现 def get_field_type_info( self, field_name: str, field_value: Any, parent_field_accessor: "FieldAccessor" ) -> Optional[TypeInfo]: """从ROS2消息定义中提取字段类型信息 使用 ROS 消息的 SLOT_TYPES 获取精确的类型信息，并通过 TypeConverter 转换为标准类型 """ # 通过 parent_field_accessor 获取 ROS 消息实例 ros_msg_instance = parent_field_accessor.value # 构建完整路径用于TypeInfo full_path = f"{parent_field_accessor.full_path_from_root}.{field_name}" # noinspection PyProtectedMember slots = ros_msg_instance._fields_and_field_types slot_types = ros_msg_instance.SLOT_TYPES # 通过 zip 找到 field_name 对应的类型定义 ros_definition_type = None for slot_name, slot_type in zip(slots, slot_types): if slot_name == field_name: ros_definition_type = slot_type break if ros_definition_type is None: raise ValueError(f"Field '{field_name}' not found in ROS message slots") # 使用 TypeConverter 转换为标准类型 standard_type = TypeConverter.rosidl_definition_to_standard(ros_definition_type) # 创建 TypeInfo type_info = TypeInfo( field_name=field_name, field_path=full_path, standard_type=standard_type, python_type=type(field_value), original_type=ros_definition_type, ) type_info.current_value = field_value # 从 rosidl 定义中提取详细类型信息（约束、数组信息等） self._extract_from_rosidl_definition(type_info) return type_info def _extract_from_rosidl_definition(self, type_info: TypeInfo) -> None: """从rosidl_parser定义中提取详细类型信息 Args: type_info: 要填充的TypeInfo对象 """ from rosidl_parser.definition import ( AbstractNestedType, Array, BasicType, BoundedSequence, BoundedString, BoundedWString, NamespacedType, UnboundedSequence, ) # 从type_info获取所需信息 definition_type = type_info.original_type get_element_type = False # 提取约束信息 if isinstance(definition_type, (BoundedString, BoundedWString)): type_info.add_constraint(ConstraintType.MAX_LENGTH, definition_type.maximum_size) elif isinstance(definition_type, Array): type_info.is_array = True type_info.array_size = definition_type.size type_info.add_constraint(ConstraintType.MIN_ITEMS, definition_type.size) type_info.add_constraint(ConstraintType.MAX_ITEMS, definition_type.size) get_element_type = True elif isinstance(definition_type, BoundedSequence): type_info.is_array = True type_info.add_constraint(ConstraintType.MAX_ITEMS, definition_type.maximum_size) get_element_type = True elif isinstance(definition_type, UnboundedSequence): type_info.is_array = True get_element_type = True elif isinstance(definition_type, BasicType): # 基础类型的约束将在 field_accessor 中自动添加 pass elif isinstance(definition_type, NamespacedType): # 对象类型，标记为对象并提取字段信息 type_info.is_object = True type_info.add_constraint(ConstraintType.TYPE_KEEP, True) # 这里可以进一步扩展来提取对象字段信息 # 提取元素类型信息 if get_element_type: if not isinstance(definition_type, AbstractNestedType): raise TypeError(f"Expected AbstractNestedType for element type extraction, got {type(definition_type)}") # 创建元素类型的TypeInfo并递归填充 std_type = TypeConverter.rosidl_definition_to_standard(definition_type.value_type) python_type = TypeConverter.standard_to_python_type(std_type) type_info.element_type_info = TypeInfo( field_name=Consts.ELEMENT_TYPE_INFO_SYMBOL, field_path=Consts.ELEMENT_TYPE_INFO_SYMBOL, standard_type=std_type, python_type=python_type, original_type=definition_type.value_type, ) self._extract_from_rosidl_definition(type_info.element_type_info)