Initial commit

unilabos/devices/temperature/sensor_node.py

@@ -0,0 +1,117 @@
+import json
+import threading,time
+
+# class TempSensorNode:
+#     def __init__(self,port,warning,address):
+#         self._value = 0.0
+#         self.warning = warning
+#         self.device_id = address
+
+#         self.hardware_interface = port
+#         # t = threading.Thread(target=self.read_temperature, daemon=True)
+#         # t.start()
+
+#     def send_command(self ,command):
+#         print('send_command---------------------')
+#         pass
+    
+#     @property
+#     def value(self) -> float:
+#         self._value = self.send_command(self.device_id)
+#         return self._value
+#     # def read_temperature(self):
+#     #     while True:
+#     #         self.value = self.send_command(self.device_id) 
+#     #         print(self.value,'-----------')
+#     #         time.sleep(1)
+
+#     def set_warning(self, warning_temp):
+#         self.warning = warning_temp
+    
+
+
+import serial
+import struct
+from rclpy.node import Node
+import rclpy
+import threading
+
+class TempSensorNode():
+    def __init__(self,port,warning,address,baudrate=9600):
+        self._value = 0.0
+        self.warning = warning
+        self.device_id = address
+        self.success = False
+    # 配置串口
+        self.hardware_interface = serial.Serial(
+            port=port,
+            baudrate=baudrate,
+            bytesize=serial.EIGHTBITS,
+            parity=serial.PARITY_NONE,
+            stopbits=serial.STOPBITS_ONE,
+            timeout=1
+        )
+        self.lock = threading.Lock()
+
+    def calculate_crc(self,data):
+        crc = 0xFFFF
+        for pos in data:
+            crc ^= pos
+            for i in range(8):
+                if (crc & 0x0001) != 0:
+                    crc >>= 1
+                    crc ^= 0xA001
+                else:
+                    crc >>= 1
+        return crc
+
+    def build_modbus_request(self, device_id, function_code, register_address, register_count):
+        request = struct.pack('>BBHH', device_id, function_code, register_address, register_count)
+        crc = self.calculate_crc(request)
+        request += struct.pack('<H', crc)
+        return request
+
+    def read_modbus_response(self,response):
+        if len(response) < 5:
+            return None
+        data = response[:-2]
+        crc_received = struct.unpack('<H', response[-2:])[0]
+        crc_calculated = self.calculate_crc(data)
+        if crc_received == crc_calculated:
+            return data[3:]
+        return None
+
+    def send_prototype_command(self ,command):
+        # with self.lock:
+            function_code = 0x04  # 读输入寄存器
+            register_address = 0x0003  # 温度高位寄存器地址
+            register_count = 0x0002  # 读取两个寄存器
+        
+            request = self.build_modbus_request(command, function_code, register_address, register_count)
+            self.hardware_interface.write(request)
+            response = self.hardware_interface.read(9)  # 读取9字节的响应
+            data = self.read_modbus_response(response)
+            if data is None:
+                return None
+
+            high_value = struct.unpack('>H', data[:2])[0]
+            low_value = struct.unpack('>H', data[2:])[0]
+
+            # 组合高位和低位并计算实际温度
+            raw_temperature = (high_value << 16) | low_value
+            if raw_temperature & 0x8000:  # 如果低位寄存器最高位为1，表示负值
+                raw_temperature -= 0x10000  # 转换为正确的负数表示
+
+            actual_temperature = raw_temperature / 10.0
+            return actual_temperature
+    
+    @property
+    def value(self) -> float:
+        self._value = self.send_prototype_command(self.device_id)
+        return self._value
+    
+    def set_warning(self, command):
+        self.success = False
+        temp = json.loads(command)["warning_temp"]
+        self.warning = round(float(temp), 1)
+        self.success = True