智能小车（八）ros实现将智能小车数据通过TCP/IP发送到上位机网关

ros实现将智能小车数据通过TCP/IP发送到上位机网关

这里主要注意:

将浮点型数据*1000转换成int型数据然后分字节储存（另取一个字节作为符号位）。没有用struct.pack进行字节流打包，原因是不同平台，字节流的打包浮点型数据不一样，又要加上包头等校验信息，所以这里分别用单字节保存在列表中，然后sock.sendall(struct.pack("B"*len(list_send),list_send))
学到的点
1、
在 Python 中，是一个解包运算符（unpacking operator）。它可以应用于可迭代对象，如列表、元组等，并将可迭代对象中的元素解包为单独的参数。

在给定的代码中，*list_send 使用解包运算符 * 将列表 list_send 中的元素作为单独的参数传递给 struct.pack 函数。这意味着 struct.pack 函数将接收列表中的每个元素作为单独的参数。如果列表 list_send 包含三个元素 [1, 2, 3]，那么 *list_send 将被解包为 1, 2, 3。

这种解包操作是为了确保 struct.pack 函数能够接收正确数量的参数，并将它们打包为字节流发送到 socket 连接。

B是无符号8位

len(list_send)是链表长度（列表中元素个数）

2、

这里sock.connect((TCP_IP, TCP_PORT))会报错异常终止程序运行我们用try except就非常好

python 复制代码

while not connect:
    try:
        sock.connect((TCP_IP, TCP_PORT))
        print("TCP/IP连接成功")
    except:
        time_connect+=1
        print("TCP/IP连接失败次数：%d" % time_connect)
        time.sleep(10)

3、def send_data(car_subscriber): 写子函数时这个形参随便写调用传入的是什么就是什么类型

4、转16位高低8位分别存储

python 复制代码

    int16_num=int(abs(raw_data.twist.linear.y)*1000)&0xFFFF
    list_raw[4]=int16_num&0xFF 
    list_raw[5]=(int16_num>>8)&0xFF

5、创建套接字固定IP和端口否则端口会自动分配

python 复制代码

local_ip='192.168.0.11'
local_port=8090
TCP_IP = '192.168.1.81'
TCP_PORT = 8090

# 创建TCP/IP客户端套接字 AF_INET是指 IPv4 地址族 参数是套接字（socket）类型，用于指定 socket 的传输协议是基于流（stream）的，也就是 TCP/IP
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind((local_ip,local_port))
# 获取实际分配的IP地址和端口号
actual_ip, actual_port = sock.getsockname()
# 打印实际分配的IP地址和端口号
print("Successfully bound to {}:{}".format(actual_ip, actual_port))

python 复制代码

#!/usr/bin/env python
# encoding: utf-8
import binascii
from sqlite3 import connect
from threading import local
import time
import rospy
from std_msgs.msg import Float32
from sensor_msgs.msg import Imu, BatteryState
import struct
import socket
from geometry_msgs.msg import Twist
from time import sleep
#当前车头方向
x_yaw=0
# 设置TCP/IP服务器地址和端口号
local_ip='192.168.0.11'
local_port=8090
TCP_IP = '192.168.1.81'
TCP_PORT = 8090
""" TCP_IP = '192.168.0.2'
TCP_PORT = 8888  """
# 创建TCP/IP客户端套接字 AF_INET是指 IPv4 地址族 参数是套接字（socket）类型，用于指定 socket 的传输协议是基于流（stream）的，也就是 TCP/IP
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind((local_ip,local_port))
# 获取实际分配的IP地址和端口号
actual_ip, actual_port = sock.getsockname()
# 打印实际分配的IP地址和端口号
print("Successfully bound to {}:{}".format(actual_ip, actual_port))
connect = False
time_connect=0
while not connect:
    try:
        sock.connect((TCP_IP, TCP_PORT))
        print("TCP/IP连接成功")
        connect=True
    except:
        time_connect+=1
        print("TCP/IP连接失败次数：%d" % time_connect)
        time.sleep(10)

# 订阅小车速度、IMU角速度、电压和电量
class RosTcpIPPublisher:
    def __init__(self):
        
        self.twist =Twist()
        self.twist.linear.x=0.0
        self.twist.linear.y=0.0
        self.twist.linear.z=0.0
        self.twist.angular.x = 0.0
        self.twist.angular.y = 0.0
        self.twist.angular.z =0.0
        self.battery = 0.0
        rospy.Subscriber('/vel_raw', Twist, self.vel_callback)
       
        rospy.Subscriber('voltage', Float32, self.battery_callback)

    def vel_callback(self, msg):
        self.twist.linear.x = msg.linear.x
        self.twist.linear.y = msg.linear.y
        self.twist.linear.z = msg.linear.z
        self.twist.angular.x = msg.angular.x
        self.twist.angular.y = msg.angular.y
        self.twist.angular.z = msg.angular.z


    def battery_callback(self, msg):
        self.battery = msg.data

# 打包数据并发送到TCP/IP服务器
def send_data(car_subscriber):
    

    while not rospy.is_shutdown():
        list_data=trans_data(car_subscriber)
        list_send=clc_data(list_data)
        # 发送数据到TCP/IP服务器
        sock.sendall(struct.pack("B"*len(list_send),*list_send))
        sleep(1)
def trans_data(raw_data):
    list_raw=[0]*19
    if raw_data.twist.linear.x>0:
        list_raw[0]=1
    else:
        list_raw[0]=0
    if raw_data.twist.linear.y>0:
        list_raw[3]=1
    else:
        list_raw[3]=0
    if raw_data.twist.linear.z>0:
        list_raw[6]=1
    else:
        list_raw[6]=0
    if raw_data.twist.angular.x>0:
        list_raw[9]=1
    else:
        list_raw[9]=0
    if raw_data.twist.angular.y>0:
        list_raw[12]=1
    else:
        list_raw[12]=0
    if raw_data.twist.angular.z>0:
        list_raw[15]=1
    else:
        list_raw[15]=0
    
    int16_num=int(abs(raw_data.twist.linear.x)*1000)&0xFFFF
    list_raw[1]=int16_num&0xFF 
    list_raw[2]=(int16_num>>8)&0xFF 
    int16_num=int(abs(raw_data.twist.linear.y)*1000)&0xFFFF
    list_raw[4]=int16_num&0xFF 
    list_raw[5]=(int16_num>>8)&0xFF 
    int16_num=int(abs(raw_data.twist.linear.z)*1000)&0xFFFF
    list_raw[7]=int16_num&0xFF 
    list_raw[8]=(int16_num>>8)&0xFF 
    int16_num=int(abs(raw_data.twist.angular.x)*1000)&0xFFFF
    list_raw[10]=int16_num&0xFF 
    list_raw[11]=(int16_num>>8)&0xFF 
    int16_num=int(abs(raw_data.twist.angular.y)*1000)&0xFFFF
    list_raw[13]=int16_num&0xFF 
    list_raw[14]=(int16_num>>8)&0xFF 
    int16_num=int(abs(raw_data.twist.angular.z)*1000)&0xFFFF
    list_raw[16]=int16_num&0xFF 
    list_raw[17]=(int16_num>>8)&0xFF 
    list_raw[18]=int(raw_data.battery*10)&0xFF
    return list_raw
def clc_data(list_data):
    count=0     
    #i=0
    total_sum=0
    current=list_data
    list_send=[0]*0x26
    list_send[0]=0xCC
    list_send[1]=0xEE
    #总长
    list_send[2]=0x26
    list_send[3]=0x00
    #8Byte
    for i in range(8):
        list_send[i+4]=0x00
    #节点类型
    list_send[12]=0x88
    list_send[13]=0x13
    #指令
    list_send[14]=0x01
    #功能字
    #车辆编号
    list_send[15]=0x01
    count=len(list_data) 
    for i in range(count):
     list_send[i+16]=list_data[i]
    #数据长度
    len_L=20
    len_H=00
    list_send[35]=len_L
    list_send[36]=len_H
    for i in range(37):
        total_sum+=list_send[i]

    total_sum=total_sum%256
    list_send[37]=total_sum
    return list_send
if __name__ == '__main__':
    rospy.init_node('ros_tcpip_publish',anonymous=False)
    try:
        car_subscriber = RosTcpIPPublisher()
        sleep(2)
        send_data(car_subscriber)
        rospy.spin()
        sock.close()
    except Exception as e:
        rospy.loginfo("ROS_TCP/IP_ERROR!!!")
        print("发生异常",str(e))