节点1： modbus通信，作为ros2 service服务器， 节点2： 作为ros2 service服务器，状态话题发布，命令话题订阅， 节点3： 状态话题订阅，命令话题发布
程序框架
节点1：
# modbus_rtu_server.py #### command lines #### # source install/setup.bash # # 查看USB串口 # ls -al /dev/ttyUSB* # # 当前用户加入用户组 # sudo usermod -a -G dialout $USER # #查看当前用户是否在设备组 # groups $USER # # 组权限更新 # newgrp dialout # # 是否被其他程序占用 # sudo lsof /dev/ttyUSB0 # ros2 run pkg_joints modbus_rtu_server --ros-args -p stopbits:=2 -p port:="/dev/ttyUSB0" # 01 (0x01): 读线圈状态 # 02 (0x02): 读离散输入 # 03 (0x03): 读保持寄存器 # 04 (0x04): 读输入寄存器 # 05 (0x05): 写单个线圈 # 06 (0x06): 写单个寄存器 # 16 (0x10): 写多个寄存器 # 读操作(01-04): 使用address和count # 写单个(05,06): 使用address和value # 写多个(16): 使用address和registers数组 #!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from pkg_interfaces.srv import ModbusRTU from pymodbus.client import ModbusSerialClient as ModbusClient from pymodbus.exceptions import ModbusException from pymodbus.pdu import ExceptionResponse import time # 在ROS2中，MultiThreadedExecutor 和 SingleThreadedExecutor # 是两种核心的任务调度器，用于管理节点回调函数的执行。 from rclpy.executors import MultiThreadedExecutor class ndModbusRTUServer(Node): def __init__(self): super().__init__('modbus_rtu_server') # 添加参数变更回调 #self.add_on_set_parameters_callback(self.param_callback) # 参数配置 self.declare_parameters( namespace='', parameters=[ ('port', '/dev/ttyUSB0'), ('method', 'rtu'), ('baudrate', 9600), ('timeout', 0.1), ('stopbits', 1), ('bytesize', 8), ('parity', "N") # 默认值设置为字符串 ] ) # # 硬编码参数 # modbus_config = { # 'port': '/dev/ttyUSB0', # 'method': 'rtu', # 'baudrate': 9600, # 'timeout': 0.1, # 'stopbits': 1, # 'bytesize': 8, # 'parity': 'N' # 确保是字符串 # } # # 初始化Modbus客户端 # self.client_mb = ModbusClient(**modbus_config) # 初始化Modbus客户端 self.client_mb = ModbusClient( method=self.get_parameter('method').value, port=self.get_parameter('port').value, baudrate=self.get_parameter('baudrate').value, timeout=self.get_parameter('timeout').value, stopbits=self.get_parameter('stopbits').value, bytesize=self.get_parameter('bytesize').value, parity=self.get_parameter('parity').value ) if not self.client_mb.connect(): self.get_logger().error("Modbus连接失败！") raise RuntimeError("Modbus连接失败") # 创建互斥回调组 self.mutex_callback_group = MutuallyExclusiveCallbackGroup() # 创建服务 self.srv = self.create_service( ModbusRTU, '/modbus_rtu_service', self.handle_request, callback_group=self.mutex_callback_group) #共享资源 串口 self.get_logger().info("Modbus RTU服务已启动...") # def param_callback(self, params): # """参数回调函数""" # results = [] # for param in params: # # 检查 parity 参数 # if param.name == 'parity': # if not isinstance(param.value, str): # # 拒绝无效类型 # result = rclpy.parameter.SetParametersResult() # result.successful = False # result.reason = f"参数 'parity' 必须是字符串类型，当前类型为 {type(param.value)}" # results.append(result) # continue # # 检查有效值 # if param.value not in ['N', 'E', 'O']: # result = rclpy.parameter.SetParametersResult() # result.successful = False # result.reason = "无效的校验位值，必须是 'N', 'E' 或 'O'" # results.append(result) # continue # # 接受其他参数 # result = rclpy.parameter.SetParametersResult(successful=True) # results.append(result) # return results def handle_request(self, request, response): result = None try: # 根据功能码处理不同请求 if request.function_code == 1: # 读线圈状态 result = self.client_mb.read_coils( address=request.address, count=request.count, slave=request.slave_id ) response.registers = [1 if bit else 0 for bit in result.bits] elif request.function_code == 2: # 读离散输入 result = self.client_mb.read_discrete_inputs( address=request.address, count=request.count, slave=request.slave_id ) response.registers = [1 if bit else 0 for bit in result.bits] elif request.function_code == 3: # 读保持寄存器 result = self.client_mb.read_holding_registers( address=request.address, count=request.count, slave=request.slave_id ) response.registers = result.registers elif request.function_code == 4: # 读输入寄存器 result = self.client_mb.read_input_registers( address=request.address, count=request.count, slave=request.slave_id ) response.registers = result.registers elif request.function_code == 5: # 写单个线圈 result = self.client_mb.write_coil( address=request.address, value=request.value, slave=request.slave_id ) response.registers = [] elif request.function_code == 6: # 写单个寄存器 result = self.client_mb.write_register( address=request.address, value=request.value, slave=request.slave_id ) response.registers = [] elif request.function_code == 16: # 写多个寄存器 result = self.client_mb.write_registers( address=request.address, values=request.registers, slave=request.slave_id ) response.registers =[] else: raise ValueError(f"不支持的功能码: {request.function_code}") # 检查Modbus异常响应 if isinstance(result, ExceptionResponse): raise ModbusException(f"从站返回异常: {result}") if result.isError(): response.success = False response.message = "{result.message}" else: response.success = True response.message = "通信成功" except ModbusException as e: response.success = False response.message = f"Modbus错误: {str(e)}" except Exception as e: response.success = False response.message = f"{str(e)} <<< {result.message}" return response def main(args=None): # rclpy.init(args=args) # nd_server = ndModbusRTUServer() # try: # rclpy.spin(nd_server) # except KeyboardInterrupt: # pass # finally: # nd_server.client.close() # nd_server.destroy_node() # rclpy.shutdown() rclpy.init(args=args) try: server = ndModbusRTUServer() executor = MultiThreadedExecutor(num_threads=4) executor.add_node(server) try: executor.spin() except KeyboardInterrupt: server.get_logger().info("Shutting down...") finally: server.destroy_node() finally: rclpy.shutdown() if __name__ == '__main__': main()
节点2：
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.qos import QoSProfile, ReliabilityPolicy,DurabilityPolicy from pkg_interfaces.srv import ModbusRTU from std_msgs.msg import String from concurrent.futures import ThreadPoolExecutor import threading from collections import deque import json import logging #设置调试信息打印 logging.basicConfig( level=logging.INFO, format='%(asctime)s.%(msecs)03d - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', #datefmt='%Y-%m-%d %H:%M:%S' datefmt='%H:%M:%S' ) class MultiRequestModbusClient(Node): def __init__(self): super().__init__('multi_request_modbus_client') # 参数配置 self.declare_parameters(namespace='', parameters=[ ('max_workers', 4), ('max_retries', 3), ('request_timeout', 2.0), ('queue_size', 100), ('qos_profile', 10) ]) # 初始化服务客户端 self.modbus_client = self.create_client( ModbusRTU, '/modbus_rtu_service', callback_group=ReentrantCallbackGroup() ) while not self.modbus_client.wait_for_service(timeout_sec=1.0): logging.info('等待Modbus服务上线...') # 消息处理队列 self.msg_queue = deque(maxlen=self.get_parameter('queue_size').value) self.queue_lock = threading.Lock() # 工作线程池（仅用于耗时操作） self.worker_pool = ThreadPoolExecutor( max_workers=self.get_parameter('max_workers').value ) # 配置QoS qos_profile = QoSProfile( reliability= ReliabilityPolicy.RELIABLE, durability = DurabilityPolicy.VOLATILE, depth= self.get_parameter('qos_profile').value ) # 创建订阅者 self.subscription = self.create_subscription( String, '/motors_commands', self.message_callback, qos_profile=qos_profile, callback_group=ReentrantCallbackGroup() ) # 发布者 # 创建发布者，发布到'/motors_status'主题，消息类型String，队列大小10 self.publisher = self.create_publisher( String, '/motors_status', qos_profile=qos_profile, callback_group=ReentrantCallbackGroup()) # 使用自定义QoS logging.info("节点初始化完成") def publish_status(self, msg_str): """发布状态方法""" msg = String() # 创建String消息 msg.data = msg_str # 设置消息数据 self.publisher.publish(msg) # 发布消息 logging.info(f'发布状态: {msg_str}') # 记录日志 def message_callback(self, msg): """非阻塞消息回调""" try: data = json.loads(msg.data) with self.queue_lock: self.msg_queue.append(data) logging.info(data) # 提交处理任务 self.worker_pool.submit(self.process_message) except json.JSONDecodeError: logging.error(f"无效JSON格式: {msg.data}") except Exception as e: logging.error(f"消息处理异常: {str(e)}") def process_message(self): """处理队列中的消息""" with self.queue_lock: if not self.msg_queue: return data = self.msg_queue.popleft() try: # 构造Modbus请求 request = ModbusRTU.Request() request.slave_id = data.get('device_id', 2) request.address = data.get('address', 0x1000) request.function_code = data.get('function_code', 3) request.count = data.get('count', 0x1a) # 异步调用服务 future = self.modbus_client.call_async(request) future.add_done_callback( lambda f: self.handle_response(f, data) ) except Exception as e: logging.error(f"请求构造失败: {str(e)}") def handle_response(self, future, original_msg): """处理服务响应""" try: response = future.result() if response.success: logging.info( f"请求成功 [ID:{original_msg.get('device_id')}] " f"数据: {response.registers}" ) self.publish_status(f"数据: {response.registers}") else: logging.warning( f"请求失败: {response.message}" ) except Exception as e: logging.error(f"响应处理异常: {str(e)}") def destroy_node(self): """安全的资源清理""" logging.info("正在关闭节点...") try: if hasattr(self, 'worker_pool'): self.worker_pool.shutdown(wait=True) except Exception as e: logging.error(f"关闭线程池时出错: {str(e)}") finally: super().destroy_node() def main(args=None): rclpy.init(args=args) try: # 创建节点 node = MultiRequestModbusClient() # 使用多线程执行器 executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) try: executor.spin() except KeyboardInterrupt: node.get_logger().info("接收到终止信号") finally: node.destroy_node() executor.shutdown() except Exception as e: print(f"节点启动失败: {str(e)}") finally: rclpy.shutdown() if __name__ == '__main__': main()
节点3：
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.qos import QoSProfile, ReliabilityPolicy,DurabilityPolicy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from std_msgs.msg import Header from std_msgs.msg import String from pkg_interfaces.srv import TrajectoryRTU import random import time import array import json from typing import Dict, Any, Union, List, Optional import logging #设置调试信息打印 logging.basicConfig( level=logging.INFO, format='%(asctime)s.%(msecs)03d - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', #datefmt='%Y-%m-%d %H:%M:%S' datefmt='%H:%M:%S' ) ################################################################################### # ROS2 消息 字典 JSON字符串 JSON文件操作与转换 class ROS2MSG_2JSON: """ 支持：ROS2消息（支持基本类型和数组）与字典、JSON转换 ROS2消息<--->字典<--->JSON字符串 必须库： import json #json数据处理 import array # 导入array模块 from typing import Dict, Any, Union, List, Optional # 类型检查 """ """ 支持：ROS2消息（支持基本类型和数组）与字典、JSON转换 ROS2消息<--->字典<--->JSON字符串 必须库： import json #json数据处理 import array # 导入array模块 from typing import Dict, Any, Union, List, Optional # 类型检查 """ @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_jsonStr(data_dict: Dict[str, Any]) -> Optional[str]: """字典转JSON字符串 Args: data_dict: 要转换的字典 Returns: JSON字符串或None(转换失败时) """ if not isinstance(data_dict, dict): return None try: # 字典转JSON（ensure_ascii=False确保支持非ASCII字符） json_str = json.dumps( data_dict, ensure_ascii=False, indent=None # 紧凑格式 ) return json_str #return json.dumps(data_dict) except (TypeError, ValueError) as e: logging.debug(f"JSON serialization error: {e}") return None @staticmethod def jsonStr_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: logging.debug(f"JSON parse error: {e}") return None except TypeError as e: logging.debug(f"Invalid JSON format: {e}") return None @staticmethod def dict_to_jsonf(file_path: str, data_dict: Dict[str, Any], indent: Optional[int] = 4) -> bool: """ 将字典保存为 JSON 文件。 Args: file_path (str): JSON 文件路径（如 "data/output.json"）。 data_dict (Dict[str, Any]): 要保存的字典数据。 indent (Optional[int]): JSON 缩进空格数，默认为 4。设为 None 取消格式化。 Returns: bool: 成功返回 True，失败返回 False。 Examples: >>> data = {"name": "张三", "age": 30} >>> dict_to_json("data.json", data) """ try: with open(file_path, "w", encoding="utf-8") as f: json.dump(data_dict, f, ensure_ascii=False, indent=indent) return True except (IOError, TypeError, json.JSONEncodeError) as e: logging.error(f"Failed to save dict to JSON: {e}", exc_info=True) return False @staticmethod def jsonf_to_dict(file_path: str) -> Optional[Dict[str, Any]]: """ 从 JSON 文件读取数据并返回字典。 Args: file_path (str): JSON 文件路径。 Returns: Optional[Dict[str, Any]]: 成功返回字典，失败返回 None。 Examples: >>> data = json_to_dict("data.json") >>> print(data) """ try: with open(file_path, "r", encoding="utf-8") as f: return json.load(f) except (FileNotFoundError, json.JSONDecodeError, UnicodeDecodeError) as e: logging.error(f"Failed to load JSON to dict: {e}", exc_info=True) return None @staticmethod def ROS2MSG_2JSON_GETCFG()-> Optional[Dict[str, Any]]: file_path = "x_y_z_claw_cfg_dict.json" recovered_dict = ROS2MSG_2JSON.jsonf_to_dict(file_path=file_path) print("Json file recovere dict:", type(recovered_dict)) return recovered_dict @staticmethod def ROS2MSG_2JSON_SETCFG(): # 按照ROS2接口文件设置 key名称 和 value类型 x_y_z_claw_cfg_dict ={ 'id_group': 1,#必须是0-255 'id_act': 2,#必须是0-255 'joint_name': ["x_axis", "y_axis", "z_axis","claw"],# 按照ROS2接口文件设置 'joint_id': [2, 3, 4, 5],#必须是1-255 'fun_code': [0, 0, 0, 0],#必须是0-255 'displace': [0.5, 0.3, 0.1, float(0x00FF)],#必须是float,带小数点 'speed_rpm': [100, 100, 100, 0],#必须是0-5000 rpm 'max_displace':[1.2, 1.8, 0.8,float(0xFF00)], #必须是float,带小数点 'min_displace':[0.0, 0.0, 0.0,float(0x00FF)], #必须是float,带小数点 # 轨迹使用 #根据零件在物流箱体内，机械爪释放零件时的点组成 "opposite_vertex_pairs": [[0., 0., 0., 0.], [1., 2., 3., 0.5]],# 物流箱 "dot_lattice": [1, 2, 3, 1],# 物流箱 #限GUI 使用 'gui_tab_mode': 'tab_auto', # 'tab_manual' } # dict保存为jsonfile file_path = "x_y_z_claw_cfg_dict.json" ROS2MSG_2JSON.dict_to_jsonf(file_path=file_path, data_dict=x_y_z_claw_cfg_dict) @staticmethod def ROS2MSG_2JSON_demo(): #ROS2MSG_2JSON.ROS2MSG_2JSON_demo() ROS2MSG_2JSON.ROS2MSG_2JSON_SETCFG() x_y_z_claw_cfg_dict =ROS2MSG_2JSON.ROS2MSG_2JSON_GETCFG() # 读取jsonfile到dict file_path = "x_y_z_claw_cfg_dict.json" recovered_dict = ROS2MSG_2JSON.jsonf_to_dict(file_path=file_path) print("Json file recovere dict:", type(recovered_dict)) print("Json file recovere dict:", recovered_dict) # 创建并初始化Request request = TrajectoryRTU.Request() ROS2MSG_2JSON.set_ros_attrs(request,data_dict= x_y_z_claw_cfg_dict) print("ROS2 interface Request:", type(request)) print("ROS2 interface Request:", request) # 转换为字典 request_dict = ROS2MSG_2JSON.ros_message_to_dict(request) print("Request as dict:", type(request_dict)) print("Request as dict:", request_dict) # 从字典恢复 recovered_request = ROS2MSG_2JSON.dict_to_ros_message(TrajectoryRTU.Request, request_dict) # 转换为JSONStr request_json = ROS2MSG_2JSON.dict_to_jsonStr(request_dict) print("Request as JSON Str:", type(request_json)) print("Request as JSON Str:", request_json) # 从JSONStr恢复 recovered_dict = ROS2MSG_2JSON.jsonStr_to_dict(request_json) print("Recovered dict:", type(recovered_dict)) print("Recovered dict:", recovered_dict) ######################################################################### # 定义状态枚举 from enum import Enum, auto class ServiceState(Enum): IDLE = auto() REQUEST = auto() RESPONSE = auto() # 伺服驱动器状态类 class ServoENUM: """伺服系统状态码定义 (使用8位二进制表示，兼容标准状态和扩展标志)""" # 基础状态（低4位） POWER_READY = 0b0000_0001 # 0001 主电源正常(就绪基础状态) ENABLED = 0b0000_0010 # 0010 伺服使能标志 INPOSITION = 0b0000_0100 # 0100 位置指令完成 HOME_COMPLETE = 0b0000_1000 # 1000 原点回归完成 # 复合状态（推荐使用组合） ## 完成，等待新命令 STATUS_READY = POWER_READY | ENABLED | INPOSITION # 0b0000_0111 (电源+使能+定位完成) ## 移动中 STATUS_MOVING = POWER_READY | ENABLED # 0b0000_0011 (电源+使能) STATUS_ERROR = 0b1000_0000 # 最高位表示错误 STATUS_TIMEOUT = 0b1000_0001 # 超时错误(含电源标志) ## CMD 命令值 CMD_NOP = 0 # 空命令，无操作 CMD_GOTO = 1 # 坐标移动命令 CMD_STAT = 2 # 查询设备状态字 CMD_POSI = 3 # 查询位置坐标 CMD_RESET = 4 # 查询设备状态字 CMD_REZP = 5 # 归零点 # 状态检查方法 @staticmethod def is_ready(status: int) -> bool: return (status & 0b0000_0111) == 0b0000_0111 @staticmethod def is_moving(status: int) -> bool: return (status & 0b0000_0011) == 0b0000_0011 and not (status & 0b1000_0000) @staticmethod def is_error(status: int) -> bool: return bool(status & 0b1000_0000) ################################################################################### # 定义状态枚举 class ServiceState(Enum): IDLE = auto() REQUEST = auto() RESPONSE = auto() class JointsServer_Test_Client(Node): def __init__(self): super().__init__('jointsserver_test_client') self.declare_parameter('qos_profile', 10) # 配置自动适应QoS qos_profile = QoSProfile( reliability= ReliabilityPolicy.RELIABLE, durability = DurabilityPolicy.VOLATILE, depth= self.get_parameter('qos_profile').value ) self.group_mutex = MutuallyExclusiveCallbackGroup() # 互斥组 # 创建发布者，发布到'/motors_status'主题，消息类型String，队列大小10 self.publisher_motor = self.create_publisher( String, '/motors_commands', qos_profile=qos_profile, callback_group=self.group_mutex) # 使用自定义QoS # 等待话题发布 while not self._topic_exists('/motors_status'): time.sleep(1) logging.info("the Topic '/motors_status' from Joints_server node doesnt exist, waiting ") # 创建订阅者，订阅'/motors_commands'主题，收到消息时调用status_callback self.subscription_motor = self.create_subscription( String, '/motors_status', self.motors_status_callback, qos_profile=qos_profile, callback_group=self.group_mutex) # 使用自定义QoS # Use a MutuallyExclusiveCallbackGroup for sequential operations self.cb_group = rclpy.callback_groups.MutuallyExclusiveCallbackGroup() self.timer = self.create_timer( 5, # Longer interval between tests self.test_jointsserver_operations, callback_group=self.cb_group ) self.test_count = 0 def _topic_exists(self, topic_name: str) -> bool: """检查 Topic 是否存在""" topic_info = self.get_topic_names_and_types() for name, _ in topic_info:# name, types in topic_info: if name == topic_name: return True return False def test_jointsserver_operations(self): self.test_count += 1 self.get_logger().info(f"\n=== Running Test #{self.test_count} ===") id_group =0 id_act = 1 joint_name =["x_axis","y_axis","z_axis","claw"] joint_id =[2,3,4,5] fun_code =[ServoENUM.CMD_POSI,ServoENUM.CMD_POSI,ServoENUM.CMD_POSI,ServoENUM.CMD_POSI] #fun_code =[ServoENUM.CMD_NOP,ServoENUM.CMD_NOP,ServoENUM.CMD_NOP,ServoENUM.CMD_NOP] displace =[0.2*(self.test_count%2)+0.4, 0.2*(self.test_count%2)+0.4, 0.0,0] speed_rpm =[200,200,100,100] request_dict ={ "id_group": id_group, "id_act": id_act, "joint_name": joint_name, "joint_id": joint_id, # uint8数组 "fun_code": fun_code, # uint8数组 "displace": displace, # float32数组 array('f',[2.0, 3.0, 1.5]) "speed_rpm": speed_rpm, # uint16数组 } # 时间戳处理（从ROS 2时间转换为uint64） now = self.get_clock().now() request_dict ["stamp_ns"] = now.nanoseconds # uint64 # 格式JSON字符串 jsonStr=ROS2MSG_2JSON.dict_to_jsonStr(request_dict ) # 发布话题 self.publish_command(jsonStr) # #self.send_request(request_dict=request_dict) def publish_command(self, msg_str): """发布命令方法""" msg = String() # 创建String消息 msg.data = msg_str # 设置消息数据 self.publisher_motor.publish(msg) # 发布消息 logging.info(f'发布命令: {msg_str}') # 记录日志 def motors_status_callback(self,msg): msg_json = msg.data # 格式JSON字符串 response_dict=ROS2MSG_2JSON.jsonStr_to_dict(msg_json) logging.info(f'{ msg_json}') def main(args=None): rclpy.init(args=args) test_client = JointsServer_Test_Client() try: rclpy.spin(test_client) except KeyboardInterrupt: test_client.get_logger().info("Keyboard interrupt, shutting down...") finally: test_client.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
节点2：标注
1 #!/usr/bin/env python3 2 import rclpy 3 from rclpy.node import Node 4 from rclpy.executors import MultiThreadedExecutor 5 from rclpy.callback_groups import ReentrantCallbackGroup 6 from rclpy.qos import QoSProfile, ReliabilityPolicy,DurabilityPolicy 7 8 from pkg_interfaces.srv import ModbusRTU 9 from std_msgs.msg import String 10 from concurrent.futures import ThreadPoolExecutor 11 import threading 12 from collections import deque 13 import json 14 15 import logging 16 #设置调试信息打印 17 logging.basicConfig( 18 level=logging.INFO, 19 format='%(asctime)s.%(msecs)03d - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', 20 #datefmt='%Y-%m-%d %H:%M:%S' 21 datefmt='%H:%M:%S' 22 ) 23 class MultiRequestModbusClient(Node): 24 def __init__(self): 25 super().__init__('multi_request_modbus_client') 26 27 # 参数配置 28 self.declare_parameters(namespace='', 29 parameters=[ 30 ('max_workers', 4), 31 ('max_retries', 3), 32 ('request_timeout', 2.0), 33 ('queue_size', 100), 34 ('qos_profile', 10) 35 ]) 36 37 # 初始化服务客户端 38 self.modbus_client = self.create_client( 39 ModbusRTU, 40 '/modbus_rtu_service', 41 callback_group=ReentrantCallbackGroup() 42 ) 43 while not self.modbus_client.wait_for_service(timeout_sec=1.0): 44 logging.info('等待Modbus服务上线...') 45 46 # 消息处理队列 47 self.msg_queue = deque(maxlen=self.get_parameter('queue_size').value) 48 self.queue_lock = threading.Lock() 49 50 # 工作线程池（仅用于耗时操作） 51 self.worker_pool = ThreadPoolExecutor( 52 max_workers=self.get_parameter('max_workers').value 53 ) 54 # 配置QoS 55 qos_profile = QoSProfile( 56 reliability= ReliabilityPolicy.RELIABLE, 57 durability = DurabilityPolicy.VOLATILE, 58 depth= self.get_parameter('qos_profile').value 59 ) 60 # 创建订阅者 61 self.subscription = self.create_subscription( 62 String, 63 '/motors_commands', 64 self.message_callback, 65 qos_profile=qos_profile, 66 callback_group=ReentrantCallbackGroup() 67 ) 68 # 发布者 69 # 创建发布者，发布到'/motors_status'主题，消息类型String，队列大小10 70 self.publisher = self.create_publisher( 71 String, 72 '/motors_status', 73 qos_profile=qos_profile, 74 callback_group=ReentrantCallbackGroup()) # 使用自定义QoS 75 76 logging.info("节点初始化完成") 77 def publish_status(self, msg_str): 78 """发布状态方法""" 79 msg = String() # 创建String消息 80 msg.data = msg_str # 设置消息数据 81 self.publisher.publish(msg) # 发布消息 82 logging.info(f'发布状态: {msg_str}') # 记录日志 83 84 def message_callback(self, msg): 85 """非阻塞消息回调""" 86 try: 87 data = json.loads(msg.data) 88 with self.queue_lock: 89 self.msg_queue.append(data) 90 logging.info(data) 91 # 提交处理任务 92 self.worker_pool.submit(self.process_message) 93 94 except json.JSONDecodeError: 95 logging.error(f"无效JSON格式: {msg.data}") 96 except Exception as e: 97 logging.error(f"消息处理异常: {str(e)}") 98 99 def process_message(self): 100 """处理队列中的消息""" 101 with self.queue_lock: 102 if not self.msg_queue: 103 return 104 data = self.msg_queue.popleft() 105 106 try: 107 # 构造Modbus请求 108 request = ModbusRTU.Request() 109 request.slave_id = data.get('device_id', 2) 110 request.address = data.get('address', 0x1000) 111 request.function_code = data.get('function_code', 3) 112 request.count = data.get('count', 0x1a) 113 114 # 异步调用服务 115 future = self.modbus_client.call_async(request) 116 future.add_done_callback( 117 lambda f: self.handle_response(f, data)#f参数代表一个已经完成的Future对象 118 ) 119 120 except Exception as e: 121 logging.error(f"请求构造失败: {str(e)}") 122 123 def handle_response(self, future, original_msg): 124 """处理服务响应""" 125 try: 126 response = future.result() 127 if response.success: 128 logging.info( 129 f"请求成功 [ID:{original_msg.get('device_id')}] " 130 f"数据: {response.registers}" 131 ) 132 self.publish_status(f"数据: {response.registers}") 133 else: 134 logging.warning( 135 f"请求失败: {response.message}" 136 ) 137 except Exception as e: 138 logging.error(f"响应处理异常: {str(e)}") 139 140 def destroy_node(self): 141 """安全的资源清理""" 142 logging.info("正在关闭节点...") 143 try: 144 if hasattr(self, 'worker_pool'): 145 self.worker_pool.shutdown(wait=True) 146 except Exception as e: 147 logging.error(f"关闭线程池时出错: {str(e)}") 148 finally: 149 super().destroy_node() 150 151 def main(args=None): 152 rclpy.init(args=args) 153 154 try: 155 # 创建节点 156 node = MultiRequestModbusClient() 157 158 # 使用多线程执行器 159 executor = MultiThreadedExecutor(num_threads=4) 160 executor.add_node(node) 161 162 try: 163 executor.spin() 164 except KeyboardInterrupt: 165 node.get_logger().info("接收到终止信号") 166 finally: 167 node.destroy_node() 168 executor.shutdown() 169 170 except Exception as e: 171 print(f"节点启动失败: {str(e)}") 172 finally: 173 rclpy.shutdown() 174 175 if __name__ == '__main__': 176 main()
模板
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.qos import QoSProfile, ReliabilityPolicy,DurabilityPolicy #from pkg_interfaces.srv import ModbusRTU # 自定义 from pkg_interfaces.srv import TrajectoryRTU # 自定义 from std_msgs.msg import String from concurrent.futures import ThreadPoolExecutor import threading from collections import deque import json import logging #设置调试信息打印 logging.basicConfig( level=logging.INFO, format='%(asctime)s.%(msecs)03d - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', #datefmt='%Y-%m-%d %H:%M:%S' datefmt='%H:%M:%S' ) class MultiRequestModbusClient(Node): """ TOPIC订阅GUI命令信息 ----> SERVICE客户端命令关节移动 ---->TOPIC发布信息至GUI""" def __init__(self): super().__init__('multi_request_modbus_client') # 核心变量示例 - 格式化整理 data_dict = { "request": { 'id_group': 1, # uint8 (0-255) 'id_act': 2, # uint8 (0-255) 'joint_name': ["x_axis", "y_axis", "z_axis", "claw"], # ROS2的关节标识符号 'joint_id': [2, 3, 4, 5], # uint8 (1-255) 'fun_code': [0, 0, 0, 0], # uint8 (0-255) 'displace': [0.5, 0.3, 0.1, float(0x00FF)], # float32 'speed_rpm': [100, 100, 100, 0], # uint16 (0-3000 RPM) # 安全限制 'max_displace': [1.2, 1.8, 0.8, float(0xFF00)], # float32 'min_displace': [0.0, 0.0, 0.0, float(0x00FF)], # float32 # 轨迹规划参数 'opposite_vertex_pairs': [ [0.0, 0.0, 0.0, 0.0], # 起始点 [1.0, 2.0, 3.0, 0.5] # 终止点 (物流箱坐标系) ], 'dot_lattice': [2, 3, 2, 1] # 物流箱栅格尺寸 }, "response": { # GUI显示数据 "displace": [0.0, 0.0, 0.0, 0.0], # 当前坐标 "status": [0, 0, 0, 0], # 设备状态码 "success": [True, False, True, False], # 操作状态 "message": ['x', 'y', 'z', 'claw'] # 调试信息 }, "pallet_gui": { # 托盘可视化数据 "box_xyz": [2, 3, 2], # [col, row, layer]，即物流箱栅格尺寸 "pos_xyz": [0, 0, 0], # 当前位置 "part_id": 0 # 物料ID } } logging.info(data_dict) """异步线程池设置""" # 消息处理队列 self.msg_queue = deque(maxlen=self.get_parameter('queue_size').value) self.queue_lock = threading.Lock() # 工作线程池（仅用于耗时操作） self.worker_pool = ThreadPoolExecutor( max_workers=self.get_parameter('max_workers').value ) # 参数配置 self.declare_parameters(namespace='', parameters=[ ('max_workers', 4), ('max_retries', 3), ('request_timeout', 2.0), ('queue_size', 100), ('qos_profile', 10) ]) """ROS2 Service Client 关节电机""" # 初始化服务客户端 # 配置QoS qos_profile = QoSProfile( reliability= ReliabilityPolicy.RELIABLE, durability = DurabilityPolicy.VOLATILE, depth= self.get_parameter('qos_profile').value ) self.modbus_client = self.create_client( String, '/joints_service', qos_profile= qos_profile, callback_group=ReentrantCallbackGroup() ) while not self.modbus_client.wait_for_service(timeout_sec=1.0): logging.info('等待Modbus服务上线...') # 配置QoS qos_profile = QoSProfile( reliability= ReliabilityPolicy.RELIABLE, durability = DurabilityPolicy.VOLATILE, depth= self.get_parameter('qos_profile').value ) # 创建订阅者 self.subscription = self.create_subscription( String, '/motors_commands', self.subscribe_message_callback, qos_profile=qos_profile, callback_group=ReentrantCallbackGroup() ) # 发布者 # 创建发布者，发布到'/motors_status'主题，消息类型String，队列大小10 self.publisher = self.create_publisher( String, '/motors_status', qos_profile=qos_profile, callback_group=ReentrantCallbackGroup()) # 使用自定义QoS logging.info("节点初始化完成") def publish_status(self, msg_str): """发布状态方法""" msg = String() # 创建String消息 msg.data = msg_str # 设置消息数据 self.publisher.publish(msg) # 发布消息 logging.info(f'发布状态: {msg_str}') # 记录日志 def subscribe_message_callback(self, msg): """非阻塞消息回调""" try: data = json.loads(msg.data) with self.queue_lock: #加入待处理队列在线程中处理 self.msg_queue.append(data) logging.info(data) # 作为线程提交处理任务进入线程池 self.worker_pool.submit(self.process_message) except json.JSONDecodeError: logging.error(f"无效JSON格式: {msg.data}") except Exception as e: logging.error(f"消息处理异常: {str(e)}") def process_message(self): """处理队列中的消息""" with self.queue_lock: if not self.msg_queue: return #待处理队列 按照FIFO取数据 data = self.msg_queue.popleft()#提取并删除第一个元素 #data = self.msg_queue[0]# 查看但不删除第一个元素 try: # 构造ROS2 SERVICE请求 request = TrajectoryRTU.Request() # request.slave_id = data.get('device_id', 2) # request.address = data.get('address', 0x1000) # request.function_code = data.get('function_code', 3) # request.count = data.get('count', 0x1a) # 异步调用服务 future = self.modbus_client.call_async(request) future.add_done_callback( lambda f: self.handle_service_response(f, data)#f参数代表一个已经完成的Future对象 ) except Exception as e: logging.error(f"请求构造失败: {str(e)}") def handle_service_response(self, future, original_msg): """处理服务响应""" try: response = future.result() if response.success: logging.info( f"请求成功 [ID:{original_msg.get('device_id')}] " f"数据: {response.registers}" ) self.publish_status(f"数据: {response.registers}") else: logging.warning( f"请求失败: {response.message}" ) except Exception as e: logging.error(f"响应处理异常: {str(e)}") def destroy_node(self): """安全的资源清理""" logging.info("正在关闭节点...") try: if hasattr(self, 'worker_pool'): self.worker_pool.shutdown(wait=True) except Exception as e: logging.error(f"关闭线程池时出错: {str(e)}") finally: super().destroy_node() def main(args=None): rclpy.init(args=args) try: # 创建节点 node = MultiRequestModbusClient() # 使用多线程执行器 executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) try: executor.spin() except KeyboardInterrupt: node.get_logger().info("接收到终止信号") finally: node.destroy_node() executor.shutdown() except Exception as e: print(f"节点启动失败: {str(e)}") finally: rclpy.shutdown() if __name__ == '__main__': main()
浙公网安备 33010602011771号