代码流程分析

于 2024-09-09 11:45:13 发布
阅读量347 收藏 4
点赞数 5
文章标签： python 开发语言 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_69531103/article/details/142004759
版权

代码流程编辑

{
def main():
    loop_counter = 0
    vehicleControl1 = vehicleControlAPI(0, 0, 0)  # 控制初始化
    socketServer = SocketServer()
    socketServer.socket_connect()

    while True:
        dataState, apiList = socketServer.socket_launch()
        if dataState:
            if apiList != None:
                if loop_counter % sceneInfoOutputGap == 1:
                    print("\n\nInfo begin:")
                    apiList.showAllState()
                    print("gear mode: ", vehicleControl1.gear)

        if dataState and loop_counter == 0:
            socketServer.socket_respond()

        elif dataState and apiList != None and apiList.messageState() and loop_counter != 0:

            roadLine = RoadLine()
            roadLineList = roadLine.trajvisualization(apiList.trajListAPI())
            vehicleControl1.__pathVisualizationSet__(roadLineList[1], roadLineList[2],
                                                     roadLineList[1], roadLineList[2])

            control_dict_demo = algorithm(apiList, vehicleControl1)
            control_dict_demo += "|end"
            socketServer.socket_send(control_dict_demo)
        loop_counter += 1
}
初始化

控制指令初始化
socketServer = SocketServer() 创建socketServer 类对象
socketServer.socket_connect() 建立连接

{
    def socket_connect(self):
        # 建立websocket连接
        try:
            x = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            x.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            x.bind(("127.0.0.1", 5061))
            x.listen(10)

            self.__conn, address = x.accept()
        except socket.error as msg:
            print(msg)
            sys.exit(1)
        return self.__conn, address
}
创建了socketServer类。类有属性conn

循环0

dataState, apiList = socketServer.socket_launch()

{
    def socket_launch(self):
        result = self.__conn.recv(4096 * 500)
        dataList = result.split(b"|end")  # 使用|end作为分隔符
        data = dataList[0]
        data_json = json.loads(data)
        apiList = None
        dataState = False
        if data != None:
            dataState = True
            print(data_json)
            if data_json['SimCarMsg'] != None:
                apiList = APIList(data_json['SimCarMsg'])

        return dataState, apiList
  }
如果成果收到数据，dataState为True，apiList = APIList(data_json[‘SimCarMsg’])
        if dataState and loop_counter == 0:
            socketServer.socket_respond()

第一轮通信返回空包

   def socket_respond(self):
        self.__conn.send(bytes('{"code":2,"UserInfo":null,"SimCarMsg":null, "messager":""}|end',
                        encoding="utf-8"))

loop_counter +=1

此时进入循环1

dataState, apiList = socketServer.socket_launch()

接收数据包，dataState为True，apiList = APIList(data_json[‘SimCarMsg’])
{
if dataState:            
	if apiList != None:
                if loop_counter % sceneInfoOutputGap == 1:
                    print("\n\nInfo begin:")
                    apiList.showAllState()
                    print("gear mode: ", vehicleControl1.gear)
}

这里为什么不改成dataState和apiList 的判断
在config里sceneInfoOutputGap = 2，是obstacle信息输出间隔
循环零不通过此处。

    def showAllState(self):
        print("DataGnssAPI", self.DataGnssAPI())
        print("DataMainVehilceAPI", self.DataMainVehilceAPI())
        print("ObstacleEntryListAPI", self.ObstacleEntryListAPI())
        print("RoadLineListAPI", self.RoadLineListAPI())
        print("TrafficLightListAPI", self.TrafficLightListAPI())
        print("trajListAPI", self.trajListAPI())

第一轮开始，发送带有数据的包

        elif dataState and apiList != None and apiList.messageState() and loop_counter != 0:

            roadLine = RoadLine()
            roadLineList = roadLine.trajvisualization(apiList.trajListAPI())
            vehicleControl1.__pathVisualizationSet__(roadLineList[1], roadLineList[2],
                                                     roadLineList[1], roadLineList[2])

            control_dict_demo = algorithm(apiList, vehicleControl1)
            control_dict_demo += "|end"
            socketServer.socket_send(control_dict_demo)

    def trajListAPI(self):
        return self.__trajList
        self.__trajList = data['Trajectory']

apiList.tragListAPI的内容

'Trajectory': {'trajectorySize': 20, 'trajectory': [{'P': {'x': 0.0, 'y': 100.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.09, 'y': 98.97, 'z': 0.0}, 'V': None}, {'P': {'x': -0.19, 'y': 97.96, 'z': 0.0}, 'V': None}, {'P': {'x': -0.28, 'y': 96.98, 'z': 0.0}, 'V': None}, {'P': {'x': -0.37, 'y': 95.99, 'z': 0.0}, 'V': None}, {'P': {'x': -0.46, 'y': 94.98, 'z': 0.0}, 'V': None}, {'P': {'x': -0.55, 'y': 93.99, 'z': 0.0}, 'V': None}, {'P': {'x': -0.64, 'y': 93.02, 'z': 0.0}, 'V': None}, {'P': {'x': -0.73, 'y': 92.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.82, 'y': 91.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.92, 'y': 90.01, 'z': 0.0}, 'V': None}, {'P': {'x': -1.01, 'y': 89.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.1, 'y': 88.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.19, 'y': 87.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.28, 'y': 86.03, 'z': 0.0}, 'V': None}, {'P': {'x': -1.37, 'y': 85.04, 'z': 0.0}, 'V': None}, {'P': {'x': -1.46, 'y': 84.06, 'z': 0.0}, 'V': None}, {'P': {'x': -1.55, 'y': 83.05, 'z': 0.0}, 'V': None}, {'P': {'x': -7.9, 'y': 30.53, 'z': 0.0}, 'V': None}, {'P': {'x': -8.0, 'y': 30.0, 'z': 0.0}, 'V': None}]}

class RoadLine:
    def __init__(self):
        self.RoadLineLen = 0
        self.RoadLineList = []
    def trajvisualization(self, RoadLineData):
        self.RoadLineLen = len(RoadLineData) # 道路参考线列表长度
        print("Roadlinedata", RoadLineData)
        roadList_xList = []  # 所有道路参考线点x坐标
        roadList_yList = []  # 所有道路参考线点y坐标
        roadList_zList = []  # 所有道路参考线点z坐标
        for i in range(self.RoadLineLen):
            roadList_x = []
            roadList_y = []
            roadList_z = []
            self.RoadLineList.append(RoadLineData[i])
            for j in range(len(RoadLineData[i]['PointPath'])):
                roadList_x.append(RoadLineData[i]['PointPath'][j]['x'])
                roadList_y.append(RoadLineData[i]['PointPath'][j]['y'])
                roadList_z.append(RoadLineData[i]['PointPath'][j]['z'])
            roadList_xList.append(roadList_x)
            roadList_yList.append(roadList_y)
            roadList_zList.append(roadList_z)
        print("roadList_xList",roadList_xList)
        print("roadList_yList",roadList_yList)
        print("roadList_zList",roadList_zList)
        return [roadList_xList, roadList_yList, roadList_zList]
        
    def trajvisualization(self, Trajectory):
        trajLen = Trajectory['trajectorySize']
        print("Trajectory", Trajectory)
        trajList_xList = []  # 所有道路参考线点x坐标
        trajList_yList = []  # 所有道路参考线点y坐标
        trajList_zList = []  # 所有道路参考线点z坐标
        for i in range(trajLen):
            trajList_xList.append(Trajectory['trajectory'][i]['P']['x'])
            trajList_yList.append(Trajectory['trajectory'][i]['P']['y'])
            trajList_zList.append(Trajectory['trajectory'][i]['P']['z'])
        print("trajList_xList", trajList_xList)
        print("trajList_yList", trajList_yList)
        print("trajList_zList", trajList_zList)
        return [trajLen, trajList_xList, trajList_yList, trajList_zList]

    def __trajListAppend(self, singleData):
        self.RoadLineList.append(singleData)

    def __trajClear(self):
        self.RoadLineList.clear()

roadLine是类对象
roadLineList 返回了车道线 的xyz坐标

对于    vehicleControl1.__pathVisualizationSet__(roadLineList[1], roadLineList[2],
                                                     roadLineList[1], roadLineList[2])

    def __pathVisualizationSet__(self, pathlistx, pathlisty, trajlistx, trajlisty):
        self.pathlistx = pathlistx
        self.pathlisty = pathlisty
        self.timeCurveListx = trajlistx
        self.timeCurveListy = trajlisty

这一步向demo里面输入了车道线/轨迹线数据

算法

 control_dict_demo = algorithm(apiList, vehicleControl1)
 def algorithm(apiList, vehicleControl1):
    control_velocity = 0
    control_steer = 0
    # 自动驾驶模式
    # pos_x, pos_y, pos_yaw, pose_v = LqrController.lqrControl(4, [curPose['posX']], [curPose['posY']])
    # for i in range(len(pos_x)): # 判断路径列表中与当前位置最近的元素
    #     if i < len(pos_x) - 1: # 防止溢出
    #         if math.sqrt(math.pow(pos_x[i] - curPose['posX'] ,2) +
    #                               math.pow(pos_y[i] - curPose['posY'], 2)) < \
    #                 math.sqrt(math.pow(pos_x[i + 1] - curPose['posX'] , 2) +
    #                                    math.pow((pos_y[i + 1] -curPose['posY']), 2)):
    #             control_steer = pos_yaw[i]
    #             control_brake = pose_v[i]
    #     else: # 最后一位
    #         control_steer = pos_yaw[-1]
    #         control_brake = pose_v[-1]
    # vehicleControl1.__steeringSet__(control_steer, yaw=curPose['oriZ'])
    # vehicleControl1.__throttleSet__(control_brake, speed=math.sqrt(
    #     math.pow(curPose['posX'], 2) + math.pow(curPose['posY'], 2)))
    # vehicleControl1.__brakeSet__(control_brake, speed=math.sqrt(
    #     math.pow(curPose['posX'], 2) + math.pow(curPose['posY'], 2)))

    # 手操模式
    vehicleControl1.__keyboardControl__()

    control_dict_demo = json_encoder(vehicleControl1)
    control_dict_demo = json.dumps(control_dict_demo)

    return control_dict_demo

先分析手操模式

json.encoder 将vehicleControl 里的数据填入control_dict_demo 中，是字典
json.dumps 用于将 Python 对象，此处为demo字典 编码成 JSON 字符串

自动驾驶模式

pos_x, pos_y, pos_yaw, pose_v = LqrController.lqrControl(4, [curPose['posX']], [curPose['posY']])

根据当前位置算出路径的位置，转向，速度
找到路径中的最近点
转向= 预期角度和当前角度之差
油门/刹车=预期速度和当前速度之差（当前速度为何是两点之间的位移）
所以我们只需要传入油门刹车转向

车辆切出

算法控制的数据包

{
"E:\work_Miao\python project\pythonProject\.venv\Scripts\python.exe" "E:\work_Miao\python project\pythonProject\autodrive_demo_user\main.py" 
E:\work_Miao\python project\pythonProject\autodrive_demo_user
{'code': 1, 'UserInfo': {'sceneName': 'scene_128', 'userName': 'scene112'}, 'SimCarMsg': {'Simdata': {'timestamp': 1725715589, 'queue': []}, 'VehicleControl': None, 'Trajectory': None, 'DataGnss': None, 'DataMainVehilce': None, 'VehicleSignalLight': None, 'ObstacleEntryList': None, 'TrafficLightList': None, 'RoadLineList': None, 'MapInfo': {'path': 'C:/MyLab/Scene/a6ea65fb-4090-418e-9c51-191b4f75e468/SmartCar_Data/StreamingAssets/mapfile', 'route': 'route', 'tpoints': 'tpoint', 'rd': 'rd', 'TileDataList': []}, 'GradingInfo': None, 'DashboardMsgTY': None, 'DashboardMsgXY': None}, 'messager': ''}
{'code': 3, 'UserInfo': None, 'SimCarMsg': {'Simdata': {'timestamp': 1725715590, 'queue': [{'timestamp': 1725715589, 'data': '(0,100,0)'}]}, 'VehicleControl': {'throttle': 0.0, 'brake': 0.0, 'steering': 0.0, 'handbrake': False, 'isManualGear': False, 'gear': 3, 'movetostart': 0, 'movetoend': 0}, 'Trajectory': {'trajectorySize': 20, 'trajectory': [{'P': {'x': 0.0, 'y': 100.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.09, 'y': 98.97, 'z': 0.0}, 'V': None}, {'P': {'x': -0.19, 'y': 97.96, 'z': 0.0}, 'V': None}, {'P': {'x': -0.28, 'y': 96.98, 'z': 0.0}, 'V': None}, {'P': {'x': -0.37, 'y': 95.99, 'z': 0.0}, 'V': None}, {'P': {'x': -0.46, 'y': 94.98, 'z': 0.0}, 'V': None}, {'P': {'x': -0.55, 'y': 93.99, 'z': 0.0}, 'V': None}, {'P': {'x': -0.64, 'y': 93.02, 'z': 0.0}, 'V': None}, {'P': {'x': -0.73, 'y': 92.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.82, 'y': 91.0, 'z': 0.0}, 'V': None}, {'P': {'x': -0.92, 'y': 90.01, 'z': 0.0}, 'V': None}, {'P': {'x': -1.01, 'y': 89.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.1, 'y': 88.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.19, 'y': 87.02, 'z': 0.0}, 'V': None}, {'P': {'x': -1.28, 'y': 86.03, 'z': 0.0}, 'V': None}, {'P': {'x': -1.37, 'y': 85.04, 'z': 0.0}, 'V': None}, {'P': {'x': -1.46, 'y': 84.06, 'z': 0.0}, 'V': None}, {'P': {'x': -1.55, 'y': 83.05, 'z': 0.0}, 'V': None}, {'P': {'x': -7.9, 'y': 30.53, 'z': 0.0}, 'V': None}, {'P': {'x': -8.0, 'y': 30.0, 'z': 0.0}, 'V': None}]}, 'DataGnss': {'poseGnss': {'posX': 0.0, 'posY': 99.833, 'posZ': -0.007, 'velX': 16.67, 'velY': 0.0, 'velZ': -0.24, 'oriX': 0.0, 'oriY': -0.002, 'oriZ': 90.0}}, 'DataMainVehilce': {'mainVehicleId': 0, 'speed': 16.67, 'gear': 3, 'throttle': 0.0, 'brake': 0.0, 'steering': 0.0, 'length': 4.814, 'width': 2.18, 'height': 1.908}, 'VehicleSignalLight': {'Signal_Light_RightBlinker': 0, 'Signal_Light_LeftBlinker': 0, 'Signal_Light_DoubleFlash': 0, 'Signal_Light_BrakeLight': 0, 'Signal_Light_FrontLight': 0, 'Signal_Light_HighBeam': 0, 'Signal_Light_BackDrive': 0}, 'ObstacleEntryList': [{'id': -1, 'viewId': -1, 'type': 7, 'posX': 0.0, 'posY': 43.6, 'posZ': 0.0, 'oriX': 0.0, 'oriY': 0.0, 'oriZ': 270.0, 'velX': 0.0, 'velY': 0.0, 'velZ': 0.0, 'length': 0.26, 'width': 0.26, 'height': 0.45, 'RedundantValue': ''}], 'TrafficLightList': [], 'RoadLineList': [], 'MapInfo': {'path': 'C:/MyLab/Scene/a6ea65fb-4090-418e-9c51-191b4f75e468/SmartCar_Data/StreamingAssets/mapfile', 'route': 'route', 'tpoints': 'tpoint', 'rd': 'rd', 'TileDataList': []}, 'GradingInfo': {'sceneid': 1, 'scenetype': '128', 'targetpoint': {'x': -8.0, 'y': 30.0, 'z': 0.0}, 'usetime': 0.02, 'totaltime': 100.0}, 'DashboardMsgTY': {'x': [1.0, 2.0, 3.0, 4.0], 'y': [1.0, 2.0, 6.0, 5.0]}, 'DashboardMsgXY': {'x': [1.0, 2.0, 3.0], 'y': [1.0, 2.0, 3.0]}}, 'messager': ''}
}
qq_2105088617
关注
Shell脚本学习-阶段一
qq_43058317的博客
06-09 812
文章目录阶段一前言Shell脚本实例1.hello world2.创建用户:2.1 通过位置变量创建用户2.2提示用户输入用户名和密码创建用户2.3读取user.txt文件中的用户名批量创建用户2.4 读取list.txt文件中的账号密码批量创建用户3.显示当前计算机中所有账户的用户名4.统计当前Linux系统中可以登陆计算机的账户数量5.找出/etc/passwd中能登陆的用户,并将对应在/etc/shadow中第二列密码提出处理6.检查当前用户是否为超级管理员7.测试用户名与密码是否正确8.将Lin..
Shell-Bash 根据日期时间统计消息行数
Younge的专栏
03-26 1536
需求：统计某个文件或者特定目录下Kafka消息一段时间范围内每个小时消息条数或者每天消息条数。一行数据行实例：2018-03-28T03：00：02 ...分析： 参数：1.指定文件的文件绝对路径           2.指定日期或者开始日期和结束日期           3.指定小时或者开始小时和结束小时           4.指定统计维度（小时或者天）逻辑：1.输出逻辑 -- 文件 + 屏幕...
shell编程中的一些运算符
最新发布
2301_77836489的博客
08-09 587
shell编程中的一些运算符
linux安装gitlab
04-07
linux安装gitlab
linux系统下如何解压RAR文件软件rarforlinux
嵌入式之斋
08-06 1572
linux系统有很多种压缩文件格式，运用自身
1 Linux挂载光盘与U盘_光盘格成类似usb怎么用linux挂载(1)
2401_83974173的博客
04-27 160
【代码】1 Linux挂载光盘与U盘_光盘格成类似usb怎么用linux挂载(1)
Linux安装gitlab教程
myITliveAAA的博客
05-24 6495
linux安装gitlab教程(详细)
Linux 下 GitLab 安装教程
热门推荐
暗诺星刻的博客
01-01 1万+
Linux 中 GitLab 安装教程 为了便于以后使用，这里先给出 GitLab 使用中的常用命令。 启动 GitLab 服务：gitlab-ctl start 关闭 GitLab 服务：gitlab-ctl stop 重启 GitLab 服务：gitlab-ctl restart 操作系统内存查看：free -m 笔者的运行环境： Linux：CentOS 8 x86_64 GitLab：gitlab-ee-14.3.6-ee.0.el8.x86_64 进行这
【shell】shel脚本开发-条件测试
用心做事，用爱生活
06-12 460
123
Shell的语句判断的判断方式
qq_20016465的博客
08-07 828
shell的语句判断 1.数字 运算符 说明 -eq 检测两个数是否相等，相等返回 true。 -ne 检测两个数是否不相等，不相等返回 true。 -gt 检测左边的数是否大于右边的，如果是，则返回 true。 -lt 检测左边的数是否小于右边的，如果是，则返回 true。 -ge 检测左边的数是否大于等于右边的，如果是，则返回 true。 -le 检测左边的数是否小于等于右边的，如果是，则返回 true。 2.字符串 判断符 说明 -n STRING 字符串长度不为零 -z STRING 字符串长度为0
shell中的变量及调用
weixin_46923884的博客
05-12 1490
shell变量 什么是变量 ·以固定名称存放 可能会变化的值 -提高脚本对任务需求 运行环境变化的适应能力 -方便在脚本中重复使用 定义/赋值/查看变量 ·定义/赋值变量 -变量名=变量值 [root@svr5~]# x=12 //定义变量x 赋值12 [root@svr5~]#var1=centos //定义变量 赋值centos 查看变量 -应用变量值：$变量名 -查看变量值：echo $变量名 ，echo ${变量名} 取消变量 ·变量失效 ...
linux 安装gitlab
一种感觉的博客
09-17 333
1.下载安装 [root@linux-node1 ~]# wget http://mirrors.aliyun.com/epel/epel-release-latest-7.noarch.rpm [root@linux-node1 ~]# wget -O /etc/yum.repos.d/CentOS-Base.repo http://mirrors.aliyun.com/repo/Centos-7.repo [root@linux-node1 /]# yum install curl policycore
Linux安装GitLab
积水成多，水到渠成
05-18 2034
公司或组织经常用Gitlab做为代码仓库，这里我也安装下Gitlab，为后面CI/CD集成打下基础，官方安装地址：https://about.gitlab.com/install/#centos-7。不过官网很多人说比较坑，这里就不用官方方法安装了。
Linux安装python2
m0_74918915的博客
04-28 1014
本书是获得了很多读者好评的Linux经典畅销书**《Linux从入门到精通》的第2版**。本书第﹖版以最新的Ubuntu 12.04为版本，循序渐进地向读者介绍了Linux 的基础应用、系统管理、网络应用、娱乐和办公、程序开发、服务器配置、系统安全等。需要《Linux入门到精通》、《linux系统移植》、《Linux驱动开发入门实战》、《Linux开源网络全栈》电子书籍及教程的工程师朋友们劳烦您转发+评论。默认 Python 2.7.13 会安装在 /usr/local/bin 目录下。
k-shell引导的复杂网络最短路径高效近似算法
实验证明，基于k-shell的最短路径近似算法在通用性上表现优秀，不仅适用于各种类型的大规模复杂网络，而且在计算效率和路径准确率方面都有显著优势。这意味着它能有效地服务于现实世界中的各种应用场景，如互联网...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值