# -*- coding: utf-8 -*-
import serial
import time
import crcmod
import rospy
from sensor_msgs.msg import Range


def calc_crc(string):
    data = bytearray.fromhex(string)
    crc = 0xFFFF
    for pos in data:
        crc ^= pos
        for i in range(8):
            if (crc & 1) != 0:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    hex_crc = hex(((crc & 0xff) << 8) + (crc >> 8)) # 返回十六进制
    return hex_crc[2:] # 返回两部分十六进制字符

msg=b'\x01\x03\x01\x01\x00\x01\xD4\x36'

rospy.init_node('ultrasonic_publisher', anonymous=True)
pub = rospy.Publisher('sonar_front_left', Range, queue_size=10)
# 打开串口
ser = serial.Serial('/dev/ttyUSB0', 115200)
# ser = serial.Serial('/dev/ttyUSB0', 115200)

# 设置超时时间为1秒
ser.timeout = 0.05


# 循环发送数据
while True:
    # 发送数据
    ser.write(msg)

    range_msg = Range()
    range_msg.header.stamp = rospy.Time.now()

    # range_msg.header.frame_id = 'ultrasonic_sensor'
    range_msg.radiation_type = Range.ULTRASOUND
    range_msg.field_of_view = 0.5  # 超声波传感器的视场角
    range_msg.min_range = 0.05  # 超声波传感器的最小测量范围
    range_msg.max_range = 2.0  # 超声波传感器的最大测量范围

    # 读取结果
    result = ser.read(7)
    if not result:
        # print("Read timeout, continue...")
        continue
    # 打印结果的hex值
    # print(result.hex())

    crc_str = result.hex()[-4:]
    source_str = result.hex()[:-4]
    calc_crc_str=calc_crc(source_str)

    if(calc_crc(source_str)!=crc_str):
        print("校验失败，丢弃")
        continue

    # 提取数据并转换为十进制数
    data_str = source_str[6:]
    decimal_value = int(data_str, 16)

    range_msg.range = decimal_value/1000  # 实际测量到的距离
    if(range_msg.range>range_msg.max_range):
        range_msg.range=range_msg.max_range

    if(range_msg.range<range_msg.min_range):
        range_msg.range=range_msg.min_range


    if(result.hex()[0:2]=='01'):
        range_msg.header.frame_id = 'sonar_front'
        print(result.hex()[0:2],":",decimal_value/10.0,"cm")
        pub.publish(range_msg)


    # 等待1秒
    time.sleep(0.01)

# 关闭串口
ser.close()