ros2 rolling 自定义的消息 <> python字典 <> Json字符串<>ROS2的std_msgs.msg 中的 String类型
TrajectoryRTU.srv
# ros2 pkg create pkg_interfaces --build-type ament_cmake --dependencies rosidl_default_generators sensor_msgs --license Apache-2.0 # 在ROS 2包的srv目录下创建 TrajectoryRTU.srv # 电机驱动的轴 绝对坐标位置控制 uint16 id_group # 动作组号 uint16 id_act # 动作编号 string[] joint_name # 关节功能名称 x_ y_ z_ uint8[] joint_id # 关节控制器RTU通信地址，可以是伺服电机控制器、步进电机控制器或远程IO模块 2 3 4 uint8[] fun_code # 自定义的功能代号 1:坐标移动命令 2:查询状态命令 3:查询坐标命令 4:设备重新启动 float32[] displace # 目标坐标，浮点数，单位 m 或 rad 或 0x00FF（OFF）0xFF00（ON） uint16[] speed_rpm # 电机转速，RPM --- float32[] displace # 当前坐标，浮点数，单位 m 或 rad 或 0x00FF（OFF）0xFF00（ON） uint8[] status # 自定义的功能代号 bool[] success # 操作是否成功 string[] message # 错误信息
关键函数代码与说明
ROS2消息转字典（支持基本类型和数组）不适用 ROS2消息内有ROSOS2消息
字典转ROS2消息（支持基本类型和数组） 不适用 字典内有字典
#!/usr/bin/env python3 #************************** # 按照如下命令行方式打开VSCODE，方便调试 # source install/setup.bash && code . #************************** from typing import Dict, Any, Union, List, Optional from pkg_interfaces.srv import TrajectoryRTU from std_msgs.msg import String import json import array class RosMessageConverter: @staticmethod def ros_message_to_dict(msg) -> Dict[str, Any]: """ROS2消息转字典（支持基本类型和数组） Args: msg: ROS2消息对象 Returns: 字典，数组自动转为list Raises: TypeError: 输入不是ROS2消息时 """ if not hasattr(msg, 'get_fields_and_field_types'): raise TypeError("Input is not a ROS2 message") result = {} # 获取消息字段信息 fields = msg.get_fields_and_field_types().keys() for field in fields: value = getattr(msg, field) # 处理数组类型（包括array.array） if isinstance(value, (list, array.array)): result[field] = list(value) # 处理基本类型 else: result[field] = value return result @staticmethod def dict_to_ros_message(msg_type, data: Dict[str, Any]): """字典转ROS2消息（支持基本类型和数组） Args: msg_type: ROS2消息类（如TrajectoryRTU.Request） data: 字典数据 Returns: 构建好的ROS2消息对象 Raises: TypeError: 输入不是ROS2消息类时 """ if not hasattr(msg_type, 'get_fields_and_field_types'): raise TypeError("Input is not a ROS2 message class") msg = msg_type() fields = msg.get_fields_and_field_types().keys() for field in fields: if field not in data: continue value = data[field] current_value = getattr(msg, field) # 处理数组字段 if isinstance(current_value, (list, array.array)): container = getattr(msg, field) del container[:] if isinstance(value, (list, tuple)): container.extend(value) # 处理基本类型 else: setattr(msg, field, value) return msg @staticmethod def set_ros_attrs( ros_msg_obj: Union[TrajectoryRTU.Request, TrajectoryRTU.Response], data_dict: Dict[str, Any] ) -> None: """安全设置ROS2消息的属性值 Args: ros_msg_obj: ROS2消息对象 data_dict: 包含属性值的字典 Raises: ValueError: 如果输入参数无效 TypeError: 如果类型不匹配 """ if not ros_msg_obj or not data_dict: raise ValueError("ros_msg_obj and data_dict cannot be None") if not isinstance(data_dict, dict): raise TypeError("data_dict must be a dictionary") for field, value in data_dict.items(): if not hasattr(ros_msg_obj, field): continue attr = getattr(ros_msg_obj, field) if isinstance(attr, (list, array.array)): if not isinstance(value, (list, tuple, array.array)): raise TypeError(f"Field {field} requires sequence type, got {type(value)}") del attr[:]#attr.clear() attr.extend(value) else: setattr(ros_msg_obj, field, value) @staticmethod def get_ros_attr( ros_msg_obj: Union[TrajectoryRTU.Request, TrajectoryRTU.Response], field: str ) -> Union[Any, List[Any]]: """安全获取ROS2消息的属性值 Args: ros_msg_obj: ROS2消息对象 field: 要获取的字段名 Returns: 字段值，数组类型会转换为list Raises: ValueError: 如果输入参数无效 AttributeError: 如果字段不存在 """ if not ros_msg_obj or not field: raise ValueError("ros_msg_obj and field cannot be None") if not hasattr(ros_msg_obj, field): raise AttributeError(f"Field {field} does not exist") attr = getattr(ros_msg_obj, field) return list(attr) if isinstance(attr, (list, array.array)) else attr @staticmethod def dict_to_json(data_dict: Dict[str, Any]) -> Optional[str]: """字典转JSON字符串 Args: data_dict: 要转换的字典 Returns: JSON字符串或None(转换失败时) """ if not isinstance(data_dict, dict): return None try: return json.dumps(data_dict) except (TypeError, ValueError) as e: print(f"JSON serialization error: {e}") return None @staticmethod def json_to_dict(json_str: str) -> Optional[Dict[str, Any]]: """JSON字符串转字典 Args: json_str: JSON格式的字符串 Returns: 字典或None(解析失败时) """ if not isinstance(json_str, str): return None try: return json.loads(json_str) except json.JSONDecodeError as e: print(f"JSON parse error: {e}") return None except TypeError as e: print(f"Invalid JSON format: {e}") return None # 使用示例 if __name__ == "__main__": # 创建并初始化Request request = TrajectoryRTU.Request() RosMessageConverter.set_ros_attrs(request, { 'id_group': 1, 'id_act': 2, 'joint_name': ["x_", "y_", "z_"], 'joint_id': [2, 3, 4], 'fun_code': [1], 'displace': [0.5, 0.3, 0.1], 'speed_rpm': [100, 100, 100] }) # 转换为字典 request_dict = RosMessageConverter.ros_message_to_dict(request) print("Request as dict:", request_dict) # 从字典恢复 recovered_request = RosMessageConverter.dict_to_ros_message(TrajectoryRTU.Request, request_dict) # 转换为JSON request_json = RosMessageConverter.dict_to_json(request_dict) print("Request as JSON:", request_json) # 从JSON恢复 recovered_dict = RosMessageConverter.json_to_dict(request_json) print("Recovered dict:", recovered_dict)
二、电机类模板
from typing import Dict, Any, Union, List, Optional import logging #设置调试信息打印 logging.basicConfig( level=logging.INFO, format='%(asctime)s - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', datefmt='%Y-%m-%d %H:%M:%S' ) class Motor_Ctl: def __init__(self,**kwargs) -> None: self.speed = kwargs.get('speed', 0) # 无效 self.displace = kwargs.get('displace', 0.0)# int(self.displace) == 0x00FF 关闭 self.acc = kwargs.get('acc', 0) # 无效 # 声明参数并根据**kwargs设置,并给定默认值 self.slave_id = kwargs.get('slave_id', 5) # 驱动器通信地址 self.ratio = kwargs.get('ratio', 1) #,无效 self.radius = kwargs.get('radius', 1) #,无效 self.pulses = kwargs.get('pulses', 1) #,无效 pass def reset_motor(self,**kwargs) -> bool: """设备初始化及消除报警""" return True def rd_status(self,**kwargs) -> Optional[Dict[str,Any]]: """读取设备状态""" pass def wr_goto(self,**kwargs) -> bool: """电机以设定加速度、最高速度，移动指定绝对坐标点""" pass def wr_srzp(self,**kwargs) -> bool: """电机搜索并停在零点""" pass def rd_position(self) ->float: """读取位置坐标""" pass def reset_sensor(self) ->float: """初始化传感器编码器及消除报警""" pass
浙公网安备 33010602011771号