智能小车（八）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就非常好

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

3、def send_data(car_subscriber): 写子函数时这个形参随便写调用传入的是什么就是什么类型
4、转16位高低8位分别存储

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

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

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))

#!/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=Trueexcept:time_connect+=1print("TCP/IP连接失败次数：%d" % time_connect)time.sleep(10)# 订阅小车速度、IMU角速度、电压和电量
class RosTcpIPPublisher:def __init__(self):self.twist =Twist()self.twist.linear.x=0.0self.twist.linear.y=0.0self.twist.linear.z=0.0self.twist.angular.x = 0.0self.twist.angular.y = 0.0self.twist.angular.z =0.0self.battery = 0.0rospy.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.xself.twist.linear.y = msg.linear.yself.twist.linear.z = msg.linear.zself.twist.angular.x = msg.angular.xself.twist.angular.y = msg.angular.yself.twist.angular.z = msg.angular.zdef 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]*19if raw_data.twist.linear.x>0:list_raw[0]=1else:list_raw[0]=0if raw_data.twist.linear.y>0:list_raw[3]=1else:list_raw[3]=0if raw_data.twist.linear.z>0:list_raw[6]=1else:list_raw[6]=0if raw_data.twist.angular.x>0:list_raw[9]=1else:list_raw[9]=0if raw_data.twist.angular.y>0:list_raw[12]=1else:list_raw[12]=0if raw_data.twist.angular.z>0:list_raw[15]=1else:list_raw[15]=0int16_num=int(abs(raw_data.twist.linear.x)*1000)&0xFFFFlist_raw[1]=int16_num&0xFF list_raw[2]=(int16_num>>8)&0xFF int16_num=int(abs(raw_data.twist.linear.y)*1000)&0xFFFFlist_raw[4]=int16_num&0xFF list_raw[5]=(int16_num>>8)&0xFF int16_num=int(abs(raw_data.twist.linear.z)*1000)&0xFFFFlist_raw[7]=int16_num&0xFF list_raw[8]=(int16_num>>8)&0xFF int16_num=int(abs(raw_data.twist.angular.x)*1000)&0xFFFFlist_raw[10]=int16_num&0xFF list_raw[11]=(int16_num>>8)&0xFF int16_num=int(abs(raw_data.twist.angular.y)*1000)&0xFFFFlist_raw[13]=int16_num&0xFF list_raw[14]=(int16_num>>8)&0xFF int16_num=int(abs(raw_data.twist.angular.z)*1000)&0xFFFFlist_raw[16]=int16_num&0xFF list_raw[17]=(int16_num>>8)&0xFF list_raw[18]=int(raw_data.battery*10)&0xFFreturn list_raw
def clc_data(list_data):count=0     #i=0total_sum=0current=list_datalist_send=[0]*0x26list_send[0]=0xCClist_send[1]=0xEE#总长list_send[2]=0x26list_send[3]=0x00#8Bytefor i in range(8):list_send[i+4]=0x00#节点类型list_send[12]=0x88list_send[13]=0x13#指令list_send[14]=0x01#功能字#车辆编号list_send[15]=0x01count=len(list_data) for i in range(count):list_send[i+16]=list_data[i]#数据长度len_L=20len_H=00list_send[35]=len_Llist_send[36]=len_Hfor i in range(37):total_sum+=list_send[i]total_sum=total_sum%256list_send[37]=total_sumreturn 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))