ROS2 rolling 两个node 擦作mobus-rtu 远程IO模块
远程IO模块资料
代码：ModbusRTU.srv
# ros2 pkg create pkg_interfaces --build-type ament_cmake --dependencies rosidl_default_generators sensor_msgs --license Apache-2.0 # 在ROS 2包的srv目录下创建 ModbusRTU.srv uint8 slave_id # 从站地址 uint16 address # 寄存器/线圈地址 uint16 count # 读取数量（用于功能码01-04） uint16 value # 写入值（用于功能码05,06） uint16[] registers # 写入的多个寄存器值（用于功能码16） uint8 function_code # Modbus功能码(1,2,3,4,5,6,16) --- uint16[] registers # 读取结果 bool success # 操作是否成功 string message # 错误信息
Node 1 代码：modbus_rtu_server.py
#!/usr/bin/env python3
# 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 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 = "通信失败" 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)}" 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()
Node 2 代码：rtu_io_test.py
#!/usr/bin/env python3
#rtu_io_test.py #************************** # 远程IO模块使用 节点 #************************** import time import math import ctypes # 用于数据类型转换 import numpy as np import inspect import threading import rclpy from rclpy.node import Node from rclpy.task import Future from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.qos import QoSProfile, QoSReliabilityPolicy from std_msgs.msg import Header from pkg_interfaces.msg import Partdetect # 确保正确导入自定义消息 from pkg_interfaces.msg import Partweight # 确保正确导入自定义消息 from pkg_interfaces.msg import Turntable # 确保正确导入自定义消息 from pkg_interfaces.srv import ModbusRTU # 替换为你的包名 from pkg_joints.actuators.actuator_base import Actuator from pkg_joints.actuators.transform_pts import Cls_pts_transform # 如何使用其他包中的类 class ServoStatus: """伺服系统状态码定义 (使用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 # 超时错误(含电源标志) # 状态检查方法 @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 REMOTE_IO_CTL:# 通过RS485遥控IO模块输入输出接点 def __init__(self, **kwargs): """ IO模块构造函数 使用 ROS2 service 实现 Modbus通信,作为客户端 """ # 初始化 Actuator 基类 self.speed = kwargs.get('speed', 0) # 无效 self.displace = kwargs.get('displace', 0.0)# int(self.displace) == 0x00FF 关闭 self.acc = kwargs.get('acc', 0) # 无效 #print(type(self.acc ),self.acc ) # 声明参数并根据**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) #,无效 #print(type(self.pulses ),self.pulses ) self.status_dict = { "RELAY01_ON" : False, #继电器1 或开关1 "RELAY02_ON" : False, #继电器2 或开关2 } self.reset =None # 复位伺服控制器或初始化伺服控制器的函数方法入口 self.modbus_srv_init =None self.modbus_srv_request =None self.debug_print = self.no_print def no_print(self, *args,**kwargs): """空函数""" pass def connect(self, **kwargs): """ROS2服务通信连接""" if self.modbus_srv_init != None: self.modbus_srv_init() def send_request(self, slave_id, address, count=0, value=0, function_code=3,registers=[]): """服务的回调函数""" if self.modbus_srv_request == None: # Create a future future = Future() result = future.result() result.success = False # 返回一个无效的result return result # 返回future.result() 实例 response =self.modbus_srv_request(slave_id=slave_id, address=address, count=count, value=value, function_code=function_code, registers=registers) return response def goto(self, **kwargs): """执行器达指定位""" self.speed = kwargs.get('speed', 0) #占位 self.displace = kwargs.get('displace', float(0x0000)) # self.acc = kwargs.get('acc', 200) #占位 code = int(self.displace) if code == 1: ret = self._write_05(reg_address=0x0000, reg_value=0xFF00)#开启 if ret: ret = self._write_05(reg_address=0x0001, reg_value=0x00FF)#关闭 elif code == 2: ret = self._write_05(reg_address=0x0000, reg_value=0x00FF)#关闭 if ret: ret = self._write_05(reg_address=0x0001, reg_value=0xFF00)#开启 else: ret = self._write_05(reg_address=0x0000, reg_value=0x00FF)#关闭 if ret: ret = self._write_05(reg_address=0x0001, reg_value=0x00FF)#关闭 if code not in [0,1,2]: self.debug_print(f"@{inspect.currentframe().f_lineno}:displace {code} is not 1,2 or 0") return True def fdbk(self, **kwargs): """执行器反馈值""" code = 0x0000 regisers = self._read_01(reg_address=0x0000, reg_count=2) try: code = regisers[0] except IndexError: return 0 self.status_dict["RELAY01_ON"] = (0b00000001&code ==0b00000001) #继电器1 或开关1 self.status_dict["RELAY02_ON"] = (0b00000010&code ==0b00000010) #继电器2 或开关2 return code def do_SHOME(self):#空，占位 pass return True def reset(self,**kwargs):#空，占位 pass return True def postion(self):#空，占位 pass return 0.0 def speed_rpm(self): # 空，占位 pass return 0.0 def _write_05(self, **kwargs): """写单个继电器，发送和接收的通信报文一样的""" reg_address = kwargs.get('reg_address', None) reg_value = kwargs.get('reg_value', None) if reg_value not in [0xFF00,0x00FF]: return False # 通信 response = self.send_request(address= reg_address,value= reg_value, function_code=0x05,slave_id=self.slave_id) while not response.success:# 阻塞 time.sleep(1) self.debug_print(f"@{inspect.currentframe().f_lineno}:modbus fun=0x05 com failure: {response.message}") return response.success def _read_01(self,reg_address =0x0000,reg_count=1): """ROS2服务,Modbus-RTU,功能码1,读取多个继电器的状态值0或1""" #在Modbus协议中,功能码0x04(十六进制表示)用于读取输入寄存器(Input Registers) ret = True while ret: # 读取输入寄存器组 response = self.send_request( slave_id=self.slave_id, address=reg_address, count= reg_count, function_code=0x01 #(0x01): 读取单个线圈 ) ret = response.success if not ret:# 阻塞 self.debug_print(f"@{inspect.currentframe().f_lineno}:Modbus fun=0x01 com failure: {response.message}") # 短暂休眠避免CPU占用过高 time.sleep(0.2) ret = True continue else: self.debug_print(f"@{inspect.currentframe().f_lineno}:Modbus fun=0x01 com success: {response.message}") break if ret: return response.registers else: return []# 失败,返回空表 @staticmethod def twos_complement_fast(bits, bit_length): """bits 补码数 bit_length位数""" sign_bit = 1 << (bit_length - 1) return (bits & (sign_bit - 1)) - (bits & sign_bit) class remote_io_node(Node): def __init__(self, **kwargs): node_name = kwargs.get('node_name', "zjoint_nd") Node.__init__(self,node_name)#super().__init__('my_python_node') if not self.has_parameter('use_sim_time'): self.declare_parameter('use_sim_time', False) else: self.set_parameters([rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, False)]) # 创建两个不同的回调组 self.group1 = MutuallyExclusiveCallbackGroup() # 互斥组 #self.group2 = ReentrantCallbackGroup() # 可重入组 self.group2 = MutuallyExclusiveCallbackGroup() # 互斥组 ############################################### # Z 轴 驱动电机 # 声明参数并根据**kwargs设置,并给定默认值 self.slave_id = kwargs.get('slave_id', 1) #,多个逗号,变成了元组# 驱动器通信地址 self.ratio = kwargs.get('ratio', 1) #,直接驱动,无减速机 self.radius = kwargs.get('radius', 0.011459155902616465) #,滑动模组,导程72mm 即周长72mm,输出轮半径 self.pulses_per_rotate = kwargs.get('pulses_per_rotate', 7200) #,# 转1周的脉冲数量 72mm/7200 0.01mm # 驱动减速电机 self.motor =REMOTE_IO_CTL(slave_id =self.slave_id ,ratio =self.ratio,radius=self.radius,pulese=self.pulses_per_rotate) # ROS2 service 实现Modbus通信,作为客户端 self.modbus_svr_cli = None # 连接 ros2 service client self.motor.modbus_srv_init = self.modbus_srv_init # 方法函数名 self.motor.modbus_srv_request = self.modbus_srv_request # 方法函数名 self.motor.debug_print = self.nd_print # 方法函数名 def modbus_srv_init(self, **kwargs): """ROS2服务通信连接""" # 作为客户端 向服务器提出请求 request,等待响应response self.modbus_svr_cli= self.create_client(ModbusRTU, '/modbus_rtu_service',callback_group=self.group2) while not self.modbus_svr_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info(f'@{inspect.currentframe().f_lineno}:Start, Waiting the on_line modbus servor ...') time.sleep(0.2)# 等0.2s self.get_logger().info(f'@{inspect.currentframe().f_lineno}:conected!') def modbus_srv_request(self, slave_id, address, count=0, value=0, function_code=3,registers=[]): """服务的回调函数""" times = 0 while times <2:# 容错 可以通信2次 request = ModbusRTU.Request() request.slave_id = slave_id request.address = address request.count = count request.value = value request.function_code = function_code request.registers = registers # 同步调用service future = self.modbus_svr_cli.call_async(request) rclpy.spin_until_future_complete(self, future) # 异步调用service # future = self.client.call_async(request) # future.add_done_callback(self.service_response_callback) #通信是否成功 response = future.result() ret = response.success if ret: break times =times +1 time.sleep(0.1) return response def nd_print(self,msg): self.get_logger().info(msg) if __name__ == "__main__": # #初始化 Node 基类 if not rclpy.ok():#在初始化节点之前,先确保rclpy已经初始化。 rclpy.init()#在初始化节点之前,先确保rclpy已经初始化。 deom_joint_nd =remote_io_node(node_name="deom_joint_nd",slave_id = 1) deom_joint_nd.motor.debug_print('test print ...') deom_joint_nd.motor.connect()# 通信ROS服务连接 time.sleep(2) code = deom_joint_nd.motor.fdbk() print(f'当前坐标:{deom_joint_nd.motor.speed_rpm()}') print(f'状态BIT值:0B{code:08b}') deom_joint_nd.motor.do_SHOME() print(deom_joint_nd.motor.speed_rpm()) code = deom_joint_nd.motor.fdbk() print(f'状态BIT值:0B{code:08b}') deom_joint_nd.motor.goto(speed=0,displace=1,acc=0) print(deom_joint_nd.motor.postion()) # 当前距离坐标
演示报文：
[2025-07-17 14:14:18.455]# RECV HEX> 01 01 00 00 00 02 BD CB [2025-07-17 14:14:18.457]# AUTO REPLY HEX> 01 01 01 01 90 48 [2025-07-17 14:14:18.549]# RECV HEX> 01 01 00 00 00 02 BD CB [2025-07-17 14:14:18.552]# AUTO REPLY HEX> 01 01 01 01 90 48 [2025-07-17 14:14:18.641]# RECV HEX> 01 05 00 00 FF 00 8C 3A [2025-07-17 14:14:18.645]# AUTO REPLY HEX> 01 05 00 00 FF 00 8C 3A [2025-07-17 14:14:18.735]# RECV HEX> 01 05 00 01 FF 00 DD FA [2025-07-17 14:14:18.736]# AUTO REPLY HEX> 01 05 00 01 FF 00 DD FA
浙公网安备 33010602011771号