Merge remote-tracking branch 'origin/master'

README.md

@@ -1,7 +1,10 @@
 # 容器占用
 vtd1 : 朴津系统室外配送机器人
-vtd2 : 国企系统室外配送机器人
-vtd3 : pjisuv
+vtd2 : 国汽系统室外配送机器人
+vtd3 : 国汽系统多功能车
+
+# 已实现的错误处理方式
+xosc-pjisuv.py
 
 
 # 一、安装 ros

src/python2/pjibot_delivery/2callback-nohup.sh

@@ -5,4 +5,4 @@ if [ ! -d "./log" ]; then
 else
     echo "Directory './log' already exists."
 fi
-nohup python2 2callback-pjibot_delivery.py > log/2callback.out 2>&1 &
+nohup python2 2callback-pjibot_delivery.py > log/2callback-pjibot_delivery.out 2>&1 &

src/python2/pjibot_delivery/2callback-pjibot_delivery.py

@@ -95,7 +95,6 @@ if __name__ == '__main__':
                         access_token = result_object1.get('data').get('accessToken')
                         # 要发送的JSON参数
                         try:
-                            # logging.info("bag文件为： %s", json_object['rosBagPath'])
                             old_date = json_object['dataName']
                             data_size = bucket.get_object_meta(json_object['rosBagPath']).content_length
                             equipment_no = json_object['equipmentNo']

src/python2/pjibot_delivery/2csv-nohup.sh

@@ -5,4 +5,4 @@ if [ ! -d "./log" ]; then
 else
     echo "Directory './log' already exists."
 fi
-nohup python2 2csv-pjibot_delivery.py > log/2csv.out 2>&1 &
+nohup python2 2csv-pjibot_delivery.py > log/2csv-pjibot_delivery.out 2>&1 &

src/python2/pjibot_delivery/2pcd-nohup.sh

@@ -5,4 +5,4 @@ if [ ! -d "./log" ]; then
 else
     echo "Directory './log' already exists."
 fi
-nohup python2 2pcd-pjibot_delivery.py > log/2pcd.out 2>&1 &
+nohup python2 2pcd-pjibot_delivery.py > log/2pcd-pjibot_delivery.out 2>&1 &

src/python2/pjibot_delivery/2simulation-nohup.sh

@@ -5,4 +5,4 @@ if [ ! -d "./log" ]; then
 else
     echo "Directory './log' already exists."
 fi
-nohup python2 2simulation-pjibot_delivery.py > log/2simulation.out 2>&1 &
+nohup python2 2simulation-pjibot_delivery.py > log/2simulation-pjibot_delivery.out 2>&1 &

src/python2/pjibot_delivery/2simulation-pjibot_delivery.py

@@ -28,24 +28,11 @@ sleep_time = 60  # 每多少秒扫描一次
 def move_xosc_before_simulation(root_path):
     try:
         xosc_path = root_path + 'scenario_orig.xosc'
-        xodr_path = root_path + xodr
-        osgb_path = root_path + osgb
         logging.info('仿真还原需要的 xosc 路径为：%s' % xosc_path)
         tree1 = ET.parse(xosc_path)
 
         root1 = tree1.getroot()
         for node0 in root1:
-            if node0.tag == 'RoadNetwork':
-                for node1 in node0:
-                    # 打印根元素的标签名
-                    if 'LogicFile' == node1.tag:
-                        shutil.copy(xodr_src, xodr_path)
-                        node1.set('filepath', xodr_path)
-                        logging.info('更新 xodr 路径为：%s' % xodr_path)
-                    if 'SceneGraphFile' == node1.tag:
-                        shutil.copy(osgb_src, osgb_path)
-                        node1.set('filepath', osgb_path)
-                        logging.info('更新 osgb 路径为：%s' % osgb_path)
             if node0.tag == 'Entities':
                 for node1 in node0:
                     if node1.get("name") == 'Ego':
@@ -110,7 +97,6 @@ if __name__ == '__main__':
     while True:
         try:
             logging.info("开始新一轮扫描")
-            local_delete_list = []
             oss_delete_list = []
             prefix_list = []
             # 2 获取已经上传完成的所有目录并分组
@@ -144,18 +130,11 @@ if __name__ == '__main__':
                         if not os.path.exists(root_path1):
                             os.makedirs(root_path1)
                         bucket.get_object_to_file(parse_prefix_full + xoscName, root_path1 + 'scenario_orig.xosc')
-                        local_delete_list.append(root_path1 + 'scenario_orig.xosc')
                         move_xosc_before_simulation(root_path1)
                         simulation(parse_prefix_full, root_path1 + 'simulation.mp4')
                 except Exception as e:
                     logging.exception("局部异常处理: %s", str(e))
             # 删除本地临时文件
-            if len(local_delete_list) > 0:
-                for local_delete in local_delete_list:
-                    try:
-                        os.remove(local_delete)
-                    except Exception as e:
-                        pass
             time.sleep(sleep_time)
         except Exception as e:
             logging.exception("全局异常处理: %s", str(e))

src/python2/pjibot_delivery/2xosc-pjibot_delivery.py

@@ -62,15 +62,15 @@ if __name__ == '__main__':
                             if '/pos_pji.csv' in str(obj3.key):
                                 csv2_done = True
                         if xosc_done:
-                            logging.info("存在 simulation.xosc(scenario.xosc): %s" % str(parse_prefix_full))
+                            # logging.info("存在 simulation.xosc(scenario.xosc): %s" % str(parse_prefix_full))
                             continue
                         if not csv1_done:
-                            logging.info("不存在 /objects_pji.csv: %s" % str(parse_prefix_full))
+                            # logging.info("不存在 /objects_pji.csv: %s" % str(parse_prefix_full))
                             continue
                         if not csv2_done:
-                            logging.info("不存在 /pos_pji.csv: %s" % str(parse_prefix_full))
+                            # logging.info("不存在 /pos_pji.csv: %s" % str(parse_prefix_full))
                             continue
-                        logging.info("需要生成 simulation.xosc: %s" % str(parse_prefix_full))
+                        logging.info("需要生成 simulation.xosc(scenario.xosc): %s" % str(parse_prefix_full))
                         local_dir_full = path1 + parse_prefix_full
                         if not os.path.exists(local_dir_full):
                             os.makedirs(local_dir_full)

src/python2/pjibot_delivery/csv-pjibot_delivery.py

@@ -47,6 +47,7 @@ def parse_csv(data_bag, parse_prefix, local_parse_dir, local_delete_list):
 
         # 生成pdf
         try:
+            # 设置调用目录
             os.chdir(path2)
             # 构造命令
             command1 = ['./pji_outdoor_real',

src/python2/pjibot_delivery/resource/bagtocsv_robot.py

@@ -1,9 +1,9 @@
 # coding: utf-8
-#!/usr/bin/env python2
+# !/usr/bin/env python2
 import os
 import rosbag
 import csv
-import math 
+import math
 import rospy
 import sys
 import time
@@ -16,203 +16,214 @@ import pandas as pd
 def quaternion_to_euler(x, y, z, w):
     # 将四元数归一化
     try:
-        length = np.sqrt(x**2 + y**2 + z**2 + w**2)
+        length = np.sqrt(x ** 2 + y ** 2 + z ** 2 + w ** 2)
         x /= length
         y /= length
         z /= length
         w /= length
-    
+
         # 计算欧拉角
-        #roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
-        #pitch = np.arcsin(2*(w*y - z*x))
-        yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))  
-        return  yaw
-    except :
+        # roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
+        # pitch = np.arcsin(2*(w*y - z*x))
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))
+        return yaw
+    except:
         return 0
 
 
-
-def parsehancheng(input_dir, output_dir):   
-    dic_trajectory = ['Time','simTime','simFrame','PointNumber','posX','posY','posZ','posH',
-    'TargetX','TargetY','TargetZ','TargetH']
-    trajectory_file = open(output_dir + "/"+"trajectory_pji.csv", 'w')
+def parsehancheng(input_dir, output_dir):
+    dic_trajectory = ['Time', 'simTime', 'simFrame', 'PointNumber', 'posX', 'posY', 'posZ', 'posH',
+                      'TargetX', 'TargetY', 'TargetZ', 'TargetH']
+    trajectory_file = open(output_dir + "/" + "trajectory_pji.csv", 'w')
     writer_trajectory = csv.writer(trajectory_file)
     writer_trajectory.writerow(dic_trajectory)
-    
-    dic_EgoState = ['Time','simTime','simFrame','posX','posY','posZ','posH','speedX','speedH','cmd_speedX','cmd_speedH','obstacle','task_error_code']
-    
-    EgoState_file = open(output_dir + "/"+"ego_pji.csv", 'w')
-    
+
+    dic_EgoState = ['Time', 'simTime', 'simFrame', 'posX', 'posY', 'posZ', 'posH', 'speedX', 'speedH', 'cmd_speedX',
+                    'cmd_speedH', 'obstacle', 'task_error_code']
+
+    EgoState_file = open(output_dir + "/" + "ego_pji.csv", 'w')
+
     writer_EgoState = csv.writer(EgoState_file)
     writer_EgoState.writerow(dic_EgoState)
 
-    dic_robot_pos = ['Time','simtime','FrameID','HeadingAngle','X', 'Y' ,'Z']
-    robot_pos_file = open(output_dir + "/"+"pos_pji.csv", 'w')
+    dic_robot_pos = ['Time', 'simtime', 'FrameID', 'HeadingAngle', 'X', 'Y', 'Z', 'latitude', 'longitude']
+    robot_pos_file = open(output_dir + "/" + "pos_pji.csv", 'w')
     writer_robot_pos = csv.writer(robot_pos_file)
     writer_robot_pos.writerow(dic_robot_pos)
 
-    dic_objects = ['Time','simtime','FrameID','playerId','posH','posX', 'posY' ,'posZ','label','dimX','dimY','dimZ',
-                   'speedX','speedY','speedZ','speedH_X','speedH_Y','speedH_Z','accelX','accelY','accelZ','accelH_X','accelH_Y','accelH_Z']
-    objects_file = open(output_dir + "/"+"objects_pji.csv", 'w')
+    dic_objects = ['Time', 'simtime', 'FrameID', 'playerId', 'posH', 'posX', 'posY', 'posZ', 'label', 'dimX', 'dimY',
+                   'dimZ',
+                   'speedX', 'speedY', 'speedZ', 'speedH_X', 'speedH_Y', 'speedH_Z', 'accelX', 'accelY', 'accelZ',
+                   'accelH_X', 'accelH_Y', 'accelH_Z']
+    objects_file = open(output_dir + "/" + "objects_pji.csv", 'w')
     writer_objects = csv.writer(objects_file)
     writer_objects.writerow(dic_objects)
 
-    frame_max=sys.maxsize
-    with rosbag.Bag(input_dir ,'r') as bag:
-        #wheel_odom_flag=False
-        #cmd_vel_flag=False
+    frame_max = sys.maxsize
+    with rosbag.Bag(input_dir, 'r') as bag:
+        # wheel_odom_flag=False
+        # cmd_vel_flag=False
         first_message_time = None
-        Frame_robot_pose=1
-        obstacle_state=0
-        speed_linear=0
-        speed_angular=0
-        cmd_speed_angular=0
-        cmd_speed_linear=0
+        Frame_robot_pose = 1
+        obstacle_state = 0
+        speed_linear = 0
+        speed_angular = 0
+        cmd_speed_angular = 0
+        cmd_speed_linear = 0
         first_message_time = None
         framenum_ego = 1
-        task_error_code=0
-        #framenum_obj = 1
-        locationsimtime=' '
-        date_time=' '
-        simFrame=1
-        ego_posx,ego_posy,ego_posz,ego_posH=0,0,0,0
-        for topic,msg,t in bag.read_messages(topics=['/obstacle_detection','/wheel_odom','/cmd_vel','/robot_pose','/tracking/objects','/nav/task_feedback_info','/robot/final_trajectory']):   
-            
-            if first_message_time is None:  
+        task_error_code = 0
+        # framenum_obj = 1
+        locationsimtime = ' '
+        date_time = ' '
+        simFrame = 1
+        latitude = 0
+        longitude = 0
+        ego_posx, ego_posy, ego_posz, ego_posH = 0, 0, 0, 0
+        for topic, msg, t in bag.read_messages(
+                topics=['/obstacle_detection', '/wheel_odom', '/cmd_vel', '/robot_pose', '/tracking/objects',
+                        '/nav/task_feedback_info', '/robot/final_trajectory', '/gnss']):
+
+            if first_message_time is None:
                 first_message_time = t
                 first_message_time = rospy.Time.to_sec(first_message_time)
                 first_message_time = datetime.fromtimestamp(first_message_time)
-            
+
+            if topic == "/gnss":
+                latitude = msg.latitude / 10000000
+                longitude = msg.longitude / 10000000
+
             if topic == "/obstacle_detection":
-                obstacle_state=msg.data
-                #print(msg.data)
+                obstacle_state = msg.data
+                # print(msg.data)
 
+            if topic == "/wheel_odom":
+                # wheel_odom_flag=True
+                speed_linear = msg.twist.twist.linear.x * 3.6
+                speed_angular = msg.twist.twist.angular.z
 
-            if topic == "/wheel_odom": 
-                #wheel_odom_flag=True 
-                speed_linear=msg.twist.twist.linear.x*3.6
-                speed_angular=msg.twist.twist.angular.z
+            if topic == "/cmd_vel":
+                # cmd_vel_flag=True
+                cmd_speed_linear = msg.linear.x * 3.6
+                cmd_speed_angular = msg.angular.z
 
-                
-            if topic == "/cmd_vel": 
-                #cmd_vel_flag=True
-                cmd_speed_linear=msg.linear.x*3.6
-                cmd_speed_angular=msg.angular.z
-                
             if topic == "/nav/task_feedback_info":
-                task_error_code=msg.task_error_code
-                
+                task_error_code = msg.task_error_code
+
             if topic == "/robot_pose":
-                
                 timestamp = rospy.Time.to_sec(t)
                 date_time = datetime.fromtimestamp(timestamp)
-                locationsimtime=(date_time-first_message_time).total_seconds()
-                ego_posx=msg.pose.position.x
+                locationsimtime = (date_time - first_message_time).total_seconds()
+                ego_posx = msg.pose.position.x
+                poseX = ego_posx
                 # poseX=ego_posx-88.96626338170609
-                poseX=ego_posx
-                ego_posy=msg.pose.position.y
+                ego_posy = msg.pose.position.y
+                poseY = ego_posy
                 # poseY=ego_posy-40.553671177476645
-                poseY=ego_posy
-                poseZ=msg.pose.position.z
-                orientationX=msg.pose.orientation.x
-                orientationY=msg.pose.orientation.y
-                orientationZ=msg.pose.orientation.z
-                orientationW=msg.pose.orientation.w
-                egoyaw=quaternion_to_euler(orientationX,orientationY,orientationZ,orientationW)
-                message_location =[date_time,locationsimtime,framenum_ego,egoyaw,poseX,poseY,poseZ]    
-                #print(f'date_time={date_time},  locationtime={locationsimtime}')
+                poseZ = msg.pose.position.z
+                orientationX = msg.pose.orientation.x
+                orientationY = msg.pose.orientation.y
+                orientationZ = msg.pose.orientation.z
+                orientationW = msg.pose.orientation.w
+                egoyaw = quaternion_to_euler(orientationX, orientationY, orientationZ, orientationW)
+                message_location = [date_time, locationsimtime, framenum_ego, egoyaw, poseX, poseY, poseZ, latitude,
+                                    longitude]
+                # print(f'date_time={date_time},  locationtime={locationsimtime}')
                 writer_robot_pos.writerow(message_location)
-                #if wheel_odom_flag and cmd_vel_flag:
-                message_EgoState =[date_time,locationsimtime,Frame_robot_pose,poseX,poseY,poseZ,egoyaw,
-                                   speed_linear,speed_angular,cmd_speed_linear,cmd_speed_angular,obstacle_state,task_error_code]
-             
+                # if wheel_odom_flag and cmd_vel_flag:
+                message_EgoState = [date_time, locationsimtime, Frame_robot_pose, poseX, poseY, poseZ, egoyaw,
+                                    speed_linear, speed_angular, cmd_speed_linear, cmd_speed_angular, obstacle_state,
+                                    task_error_code]
+
                 writer_EgoState.writerow(message_EgoState)
-                framenum_ego+=1
-                Frame_robot_pose+=1
+                framenum_ego += 1
+                Frame_robot_pose += 1
 
-        
             if topic == "/tracking/objects":
                 msg_l = len(msg.objects)
                 if msg_l and (locationsimtime != ' ') and (date_time != ' '):
-                    #timestamp = rospy.Time.to_sec(t)
-                    #date_time = datetime.fromtimestamp(timestamp)
-                    #simtime=(date_time-first_message_time).total_seconds()
+                    # timestamp = rospy.Time.to_sec(t)
+                    # date_time = datetime.fromtimestamp(timestamp)
+                    # simtime=(date_time-first_message_time).total_seconds()
                     for i in range(msg_l):
                         obj_ID = msg.objects[i].id
-                        # obj_x = msg.objects[i].pose.position.x-88.96626338170609
                         obj_x = msg.objects[i].pose.position.x
-                        # obj_y = msg.objects[i].pose.position.y-40.553671177476645
+                        # obj_x = msg.objects[i].pose.position.x-88.96626338170609
                         obj_y = msg.objects[i].pose.position.y
+                        # obj_y = msg.objects[i].pose.position.y-40.553671177476645
                         obj_z = msg.objects[i].pose.position.z
-                        obj_orientationX=msg.objects[i].pose.orientation.x
-                        obj_orientationY=msg.objects[i].pose.orientation.y
-                        obj_orientationZ=msg.objects[i].pose.orientation.z
-                        obj_orientationW=msg.objects[i].pose.orientation.w
-                        objyaw=quaternion_to_euler(obj_orientationX,obj_orientationY,obj_orientationZ,obj_orientationW)
-                        obj_lable=msg.objects[i].label
-                        obj_dimX=msg.objects[i].dimensions.x
-                        obj_dimY=msg.objects[i].dimensions.y
-                        obj_dimZ=msg.objects[i].dimensions.z
-                        velocity_linear_x=msg.objects[i].velocity.linear.x
-                        velocity_linear_y=msg.objects[i].velocity.linear.y
-                        velocity_linear_z=msg.objects[i].velocity.linear.z
-                        
-                        velocity_angular_x=msg.objects[i].velocity.angular.x
-                        velocity_angular_y=msg.objects[i].velocity.angular.y
-                        velocity_angular_z=msg.objects[i].velocity.angular.z
-                        
-                        acceleration_linear_x=msg.objects[i].acceleration.linear.x
-                        acceleration_linear_y=msg.objects[i].acceleration.linear.y
-                        acceleration_linear_z=msg.objects[i].acceleration.linear.z
-                        
-                        acceleration_angular_x=msg.objects[i].acceleration.angular.x
-                        acceleration_angular_y=msg.objects[i].acceleration.angular.y
-                        acceleration_angular_z=msg.objects[i].acceleration.angular.z
-                        
-                        message_obj =[date_time,locationsimtime,Frame_robot_pose,obj_ID,objyaw,obj_x,obj_y,obj_z,obj_lable,obj_dimX,obj_dimY,obj_dimZ,
-                                      velocity_linear_x,velocity_linear_y,velocity_linear_z,velocity_angular_x,velocity_angular_y,
-                                      velocity_angular_z,acceleration_linear_x,acceleration_linear_y,acceleration_linear_z,
-                                      acceleration_angular_x,acceleration_angular_y,acceleration_angular_z]
-                        #print(f'{date_time},  {locationsimtime}')
+                        obj_orientationX = msg.objects[i].pose.orientation.x
+                        obj_orientationY = msg.objects[i].pose.orientation.y
+                        obj_orientationZ = msg.objects[i].pose.orientation.z
+                        obj_orientationW = msg.objects[i].pose.orientation.w
+                        objyaw = quaternion_to_euler(obj_orientationX, obj_orientationY, obj_orientationZ,
+                                                     obj_orientationW)
+                        obj_lable = msg.objects[i].label
+                        obj_dimX = msg.objects[i].dimensions.x
+                        obj_dimY = msg.objects[i].dimensions.y
+                        obj_dimZ = msg.objects[i].dimensions.z
+                        velocity_linear_x = msg.objects[i].velocity.linear.x
+                        velocity_linear_y = msg.objects[i].velocity.linear.y
+                        velocity_linear_z = msg.objects[i].velocity.linear.z
+
+                        velocity_angular_x = msg.objects[i].velocity.angular.x
+                        velocity_angular_y = msg.objects[i].velocity.angular.y
+                        velocity_angular_z = msg.objects[i].velocity.angular.z
+
+                        acceleration_linear_x = msg.objects[i].acceleration.linear.x
+                        acceleration_linear_y = msg.objects[i].acceleration.linear.y
+                        acceleration_linear_z = msg.objects[i].acceleration.linear.z
+
+                        acceleration_angular_x = msg.objects[i].acceleration.angular.x
+                        acceleration_angular_y = msg.objects[i].acceleration.angular.y
+                        acceleration_angular_z = msg.objects[i].acceleration.angular.z
+
+                        message_obj = [date_time, locationsimtime, Frame_robot_pose, obj_ID, objyaw, obj_x, obj_y,
+                                       obj_z, obj_lable, obj_dimX, obj_dimY, obj_dimZ,
+                                       velocity_linear_x, velocity_linear_y, velocity_linear_z, velocity_angular_x,
+                                       velocity_angular_y,
+                                       velocity_angular_z, acceleration_linear_x, acceleration_linear_y,
+                                       acceleration_linear_z,
+                                       acceleration_angular_x, acceleration_angular_y, acceleration_angular_z]
+                        # print(f'{date_time},  {locationsimtime}')
                         writer_objects.writerow(message_obj)
-                    #framenum_obj+=1 
+                    # framenum_obj+=1
             if topic == "/robot/final_trajectory":
-                PointNumber=1
+                PointNumber = 1
                 timestamp1 = rospy.Time.to_sec(t)
                 date_time1 = datetime.fromtimestamp(timestamp1)
-                locationsimtime1=(date_time1-first_message_time).total_seconds()
-                if ego_posx!=0 or ego_posy!=0:
+                locationsimtime1 = (date_time1 - first_message_time).total_seconds()
+                if ego_posx != 0 or ego_posy != 0:
                     for i in range(len(msg.waypoints)):
-                        Targetx=msg.waypoints[i].pose.pose.position.x
-                        Targety=msg.waypoints[i].pose.pose.position.y
-                        Targetz=msg.waypoints[i].pose.pose.position.z
-                        orientationX=msg.waypoints[i].pose.pose.orientation.x
-                        orientationY=msg.waypoints[i].pose.pose.orientation.y
-                        orientationZ=msg.waypoints[i].pose.pose.orientation.z
-                        orientationW=msg.waypoints[i].pose.pose.orientation.w
-                        TargetH=quaternion_to_euler(orientationX,orientationY,orientationZ,orientationW)
-                        message_trajectory =[date_time1,locationsimtime1,simFrame,PointNumber,ego_posx,ego_posy,poseZ,egoyaw,Targetx,Targety,Targetz,TargetH]
+                        Targetx = msg.waypoints[i].pose.pose.position.x
+                        Targety = msg.waypoints[i].pose.pose.position.y
+                        Targetz = msg.waypoints[i].pose.pose.position.z
+                        orientationX = msg.waypoints[i].pose.pose.orientation.x
+                        orientationY = msg.waypoints[i].pose.pose.orientation.y
+                        orientationZ = msg.waypoints[i].pose.pose.orientation.z
+                        orientationW = msg.waypoints[i].pose.pose.orientation.w
+                        TargetH = quaternion_to_euler(orientationX, orientationY, orientationZ, orientationW)
+                        message_trajectory = [date_time1, locationsimtime1, simFrame, PointNumber, ego_posx, ego_posy,
+                                              poseZ, egoyaw, Targetx, Targety, Targetz, TargetH]
                         writer_trajectory.writerow(message_trajectory)
-                        PointNumber+=1
-                    simFrame+=1
-                
+                        PointNumber += 1
+                    simFrame += 1
+
         robot_pos_file.close()
         objects_file.close()
         EgoState_file.close()
         trajectory_file.close()
-        
-        
+
+
 # if __name__ == '__main__':
 def parse(input_dir, output_dir):
-   #input_dir='/media/dell/HIKSEMI/pji_DGNC/pjioutrobot_2024-08-21-15-12-04.bag'
-   #output_dir='/media/dell/HIKSEMI/pji_DGNC'
-   # input_dir=sys.argv[1]
-   # output_dir = sys.argv[2]
-   bagname=input_dir.split('/')[-1].split('.')[0]
-
-   
-   output_dir=os.path.join(output_dir, bagname)
-   if not os.path.exists(output_dir):
-       os.makedirs(output_dir)
-   parsehancheng(input_dir, output_dir)
+    # input_dir='/media/dell/HIKSEMI/pji_DGNC/pjioutrobot_2024-08-21-15-12-04.bag'
+    # output_dir='/media/dell/HIKSEMI/pji_DGNC'
+    # input_dir=sys.argv[1]
+    # output_dir = sys.argv[2]
+    bagname = input_dir.split('/')[-1].split('.')[0]
 
+    output_dir = os.path.join(output_dir, bagname)
+    if not os.path.exists(output_dir):
+        os.makedirs(output_dir)
+    parsehancheng(input_dir, output_dir)

src/python2/pjibot_delivery/simulation-pjibot_delivery.py

@@ -28,24 +28,11 @@ sleep_time = 60  # 每多少秒扫描一次
 def move_xosc_before_simulation(root_path):
     try:
         xosc_path = root_path + 'scenario_orig.xosc'
-        xodr_path = root_path + xodr
-        osgb_path = root_path + osgb
         logging.info('仿真还原需要的 xosc 路径为：%s' % xosc_path)
         tree1 = ET.parse(xosc_path)
 
         root1 = tree1.getroot()
         for node0 in root1:
-            if node0.tag == 'RoadNetwork':
-                for node1 in node0:
-                    # 打印根元素的标签名
-                    if 'LogicFile' == node1.tag:
-                        shutil.copy(xodr_src, xodr_path)
-                        node1.set('filepath', xodr_path)
-                        logging.info('更新 xodr 路径为：%s' % xodr_path)
-                    if 'SceneGraphFile' == node1.tag:
-                        shutil.copy(osgb_src, osgb_path)
-                        node1.set('filepath', osgb_path)
-                        logging.info('更新 osgb 路径为：%s' % osgb_path)
             if node0.tag == 'Entities':
                 for node1 in node0:
                     if node1.get("name") == 'Ego':
@@ -101,15 +88,6 @@ def is_upload_completed(bucket, prefix):
     return int(count) == int(target_number)
 
 
-'''
-cname：http://open-bucket.oss.icvdc.com
-keyid：n8glvFGS25MrLY7j
-secret：xZ2Fozoarpfw0z28FUhtg8cu0yDc5d
-oss桶名： oss://open-bucket
-
-oss桶名： open-bucket
-内网endpoint: oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com
-'''
 # ------- 获取合并之后的bag包，解析出csv -------
 if __name__ == '__main__':
     # 1 创建阿里云对象
@@ -119,7 +97,6 @@ if __name__ == '__main__':
     while True:
         try:
             logging.info("开始新一轮扫描")
-            local_delete_list = []
             oss_delete_list = []
             prefix_list = []
             # 2 获取已经上传完成的所有目录并分组
@@ -153,18 +130,10 @@ if __name__ == '__main__':
                         if not os.path.exists(root_path1):
                             os.makedirs(root_path1)
                         bucket.get_object_to_file(parse_prefix_full + xoscName, root_path1 + 'scenario_orig.xosc')
-                        local_delete_list.append(root_path1 + 'scenario_orig.xosc')
                         move_xosc_before_simulation(root_path1)
                         simulation(parse_prefix_full, root_path1 + 'simulation.mp4')
                 except Exception as e:
                     logging.exception("局部异常处理: %s", str(e))
-            # 删除本地临时文件
-            if len(local_delete_list) > 0:
-                for local_delete in local_delete_list:
-                    try:
-                        os.remove(local_delete)
-                    except Exception as e:
-                        pass
             time.sleep(sleep_time)
         except Exception as e:
             logging.exception("全局异常处理: %s", str(e))

src/python2/pjibot_patrol/resource/bagtocsv_robot.py

@@ -1,9 +1,9 @@
 # coding: utf-8
-#!/usr/bin/env python2
+# !/usr/bin/env python2
 import os
 import rosbag
 import csv
-import math 
+import math
 import rospy
 import sys
 import time
@@ -16,199 +16,214 @@ import pandas as pd
 def quaternion_to_euler(x, y, z, w):
     # 将四元数归一化
     try:
-        length = np.sqrt(x**2 + y**2 + z**2 + w**2)
+        length = np.sqrt(x ** 2 + y ** 2 + z ** 2 + w ** 2)
         x /= length
         y /= length
         z /= length
         w /= length
-    
+
         # 计算欧拉角
-        #roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
-        #pitch = np.arcsin(2*(w*y - z*x))
-        yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))  
-        return  yaw
-    except :
+        # roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
+        # pitch = np.arcsin(2*(w*y - z*x))
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))
+        return yaw
+    except:
         return 0
 
 
-
-def parsehancheng(input_dir, output_dir):   
-    dic_trajectory = ['Time','simTime','simFrame','PointNumber','posX','posY','posZ','posH',
-    'TargetX','TargetY','TargetZ','TargetH']
-    trajectory_file = open(output_dir + "/"+"trajectory_pji.csv", 'w')
+def parsehancheng(input_dir, output_dir):
+    dic_trajectory = ['Time', 'simTime', 'simFrame', 'PointNumber', 'posX', 'posY', 'posZ', 'posH',
+                      'TargetX', 'TargetY', 'TargetZ', 'TargetH']
+    trajectory_file = open(output_dir + "/" + "trajectory_pji.csv", 'w')
     writer_trajectory = csv.writer(trajectory_file)
     writer_trajectory.writerow(dic_trajectory)
-    
-    dic_EgoState = ['Time','simTime','simFrame','posX','posY','posZ','posH','speedX','speedH','cmd_speedX','cmd_speedH','obstacle','task_error_code']
-    
-    EgoState_file = open(output_dir + "/"+"ego_pji.csv", 'w')
-    
+
+    dic_EgoState = ['Time', 'simTime', 'simFrame', 'posX', 'posY', 'posZ', 'posH', 'speedX', 'speedH', 'cmd_speedX',
+                    'cmd_speedH', 'obstacle', 'task_error_code']
+
+    EgoState_file = open(output_dir + "/" + "ego_pji.csv", 'w')
+
     writer_EgoState = csv.writer(EgoState_file)
     writer_EgoState.writerow(dic_EgoState)
 
-    dic_robot_pos = ['Time','simtime','FrameID','HeadingAngle','X', 'Y' ,'Z']
-    robot_pos_file = open(output_dir + "/"+"pos_pji.csv", 'w')
+    dic_robot_pos = ['Time', 'simtime', 'FrameID', 'HeadingAngle', 'X', 'Y', 'Z', 'latitude', 'longitude']
+    robot_pos_file = open(output_dir + "/" + "pos_pji.csv", 'w')
     writer_robot_pos = csv.writer(robot_pos_file)
     writer_robot_pos.writerow(dic_robot_pos)
 
-    dic_objects = ['Time','simtime','FrameID','playerId','posH','posX', 'posY' ,'posZ','label','dimX','dimY','dimZ',
-                   'speedX','speedY','speedZ','speedH_X','speedH_Y','speedH_Z','accelX','accelY','accelZ','accelH_X','accelH_Y','accelH_Z']
-    objects_file = open(output_dir + "/"+"objects_pji.csv", 'w')
+    dic_objects = ['Time', 'simtime', 'FrameID', 'playerId', 'posH', 'posX', 'posY', 'posZ', 'label', 'dimX', 'dimY',
+                   'dimZ',
+                   'speedX', 'speedY', 'speedZ', 'speedH_X', 'speedH_Y', 'speedH_Z', 'accelX', 'accelY', 'accelZ',
+                   'accelH_X', 'accelH_Y', 'accelH_Z']
+    objects_file = open(output_dir + "/" + "objects_pji.csv", 'w')
     writer_objects = csv.writer(objects_file)
     writer_objects.writerow(dic_objects)
 
-    frame_max=sys.maxsize
-    with rosbag.Bag(input_dir ,'r') as bag:
-        #wheel_odom_flag=False
-        #cmd_vel_flag=False
+    frame_max = sys.maxsize
+    with rosbag.Bag(input_dir, 'r') as bag:
+        # wheel_odom_flag=False
+        # cmd_vel_flag=False
         first_message_time = None
-        Frame_robot_pose=1
-        obstacle_state=0
-        speed_linear=0
-        speed_angular=0
-        cmd_speed_angular=0
-        cmd_speed_linear=0
+        Frame_robot_pose = 1
+        obstacle_state = 0
+        speed_linear = 0
+        speed_angular = 0
+        cmd_speed_angular = 0
+        cmd_speed_linear = 0
         first_message_time = None
         framenum_ego = 1
-        task_error_code=0
-        #framenum_obj = 1
-        locationsimtime=' '
-        date_time=' '
-        simFrame=1
-        ego_posx,ego_posy,ego_posz,ego_posH=0,0,0,0
-        for topic,msg,t in bag.read_messages(topics=['/obstacle_detection','/wheel_odom','/cmd_vel','/robot_pose','/tracking/objects','/nav/task_feedback_info','/robot/final_trajectory']):   
-            
-            if first_message_time is None:  
+        task_error_code = 0
+        # framenum_obj = 1
+        locationsimtime = ' '
+        date_time = ' '
+        simFrame = 1
+        latitude = 0
+        longitude = 0
+        ego_posx, ego_posy, ego_posz, ego_posH = 0, 0, 0, 0
+        for topic, msg, t in bag.read_messages(
+                topics=['/obstacle_detection', '/wheel_odom', '/cmd_vel', '/robot_pose', '/tracking/objects',
+                        '/nav/task_feedback_info', '/robot/final_trajectory', '/gnss']):
+
+            if first_message_time is None:
                 first_message_time = t
                 first_message_time = rospy.Time.to_sec(first_message_time)
                 first_message_time = datetime.fromtimestamp(first_message_time)
-            
+
+            if topic == "/gnss":
+                latitude = msg.latitude / 10000000
+                longitude = msg.longitude / 10000000
+
             if topic == "/obstacle_detection":
-                obstacle_state=msg.data
-                #print(msg.data)
+                obstacle_state = msg.data
+                # print(msg.data)
 
+            if topic == "/wheel_odom":
+                # wheel_odom_flag=True
+                speed_linear = msg.twist.twist.linear.x * 3.6
+                speed_angular = msg.twist.twist.angular.z
 
-            if topic == "/wheel_odom": 
-                #wheel_odom_flag=True 
-                speed_linear=msg.twist.twist.linear.x*3.6
-                speed_angular=msg.twist.twist.angular.z
+            if topic == "/cmd_vel":
+                # cmd_vel_flag=True
+                cmd_speed_linear = msg.linear.x * 3.6
+                cmd_speed_angular = msg.angular.z
 
-                
-            if topic == "/cmd_vel": 
-                #cmd_vel_flag=True
-                cmd_speed_linear=msg.linear.x*3.6
-                cmd_speed_angular=msg.angular.z
-                
             if topic == "/nav/task_feedback_info":
-                task_error_code=msg.task_error_code
-                
+                task_error_code = msg.task_error_code
+
             if topic == "/robot_pose":
-                
                 timestamp = rospy.Time.to_sec(t)
                 date_time = datetime.fromtimestamp(timestamp)
-                locationsimtime=(date_time-first_message_time).total_seconds()
-                ego_posx=msg.pose.position.x
-                poseX=ego_posx-88.96626338170609
-                ego_posy=msg.pose.position.y
-                poseY=ego_posy-40.553671177476645
-                poseZ=msg.pose.position.z
-                orientationX=msg.pose.orientation.x
-                orientationY=msg.pose.orientation.y
-                orientationZ=msg.pose.orientation.z
-                orientationW=msg.pose.orientation.w
-                egoyaw=quaternion_to_euler(orientationX,orientationY,orientationZ,orientationW)
-                message_location =[date_time,locationsimtime,framenum_ego,egoyaw,poseX,poseY,poseZ]    
-                #print(f'date_time={date_time},  locationtime={locationsimtime}')
+                locationsimtime = (date_time - first_message_time).total_seconds()
+                ego_posx = msg.pose.position.x
+                poseX = ego_posx
+                # poseX=ego_posx-88.96626338170609
+                ego_posy = msg.pose.position.y
+                poseY = ego_posy
+                # poseY=ego_posy-40.553671177476645
+                poseZ = msg.pose.position.z
+                orientationX = msg.pose.orientation.x
+                orientationY = msg.pose.orientation.y
+                orientationZ = msg.pose.orientation.z
+                orientationW = msg.pose.orientation.w
+                egoyaw = quaternion_to_euler(orientationX, orientationY, orientationZ, orientationW)
+                message_location = [date_time, locationsimtime, framenum_ego, egoyaw, poseX, poseY, poseZ, latitude,
+                                    longitude]
+                # print(f'date_time={date_time},  locationtime={locationsimtime}')
                 writer_robot_pos.writerow(message_location)
-                #if wheel_odom_flag and cmd_vel_flag:
-                message_EgoState =[date_time,locationsimtime,Frame_robot_pose,poseX,poseY,poseZ,egoyaw,
-                                   speed_linear,speed_angular,cmd_speed_linear,cmd_speed_angular,obstacle_state,task_error_code]
-             
+                # if wheel_odom_flag and cmd_vel_flag:
+                message_EgoState = [date_time, locationsimtime, Frame_robot_pose, poseX, poseY, poseZ, egoyaw,
+                                    speed_linear, speed_angular, cmd_speed_linear, cmd_speed_angular, obstacle_state,
+                                    task_error_code]
+
                 writer_EgoState.writerow(message_EgoState)
-                framenum_ego+=1
-                Frame_robot_pose+=1
+                framenum_ego += 1
+                Frame_robot_pose += 1
 
-        
             if topic == "/tracking/objects":
                 msg_l = len(msg.objects)
                 if msg_l and (locationsimtime != ' ') and (date_time != ' '):
-                    #timestamp = rospy.Time.to_sec(t)
-                    #date_time = datetime.fromtimestamp(timestamp)
-                    #simtime=(date_time-first_message_time).total_seconds()
+                    # timestamp = rospy.Time.to_sec(t)
+                    # date_time = datetime.fromtimestamp(timestamp)
+                    # simtime=(date_time-first_message_time).total_seconds()
                     for i in range(msg_l):
                         obj_ID = msg.objects[i].id
-                        obj_x = msg.objects[i].pose.position.x-88.96626338170609
-                        obj_y = msg.objects[i].pose.position.y-40.553671177476645
+                        obj_x = msg.objects[i].pose.position.x
+                        # obj_x = msg.objects[i].pose.position.x-88.96626338170609
+                        obj_y = msg.objects[i].pose.position.y
+                        # obj_y = msg.objects[i].pose.position.y-40.553671177476645
                         obj_z = msg.objects[i].pose.position.z
-                        obj_orientationX=msg.objects[i].pose.orientation.x
-                        obj_orientationY=msg.objects[i].pose.orientation.y
-                        obj_orientationZ=msg.objects[i].pose.orientation.z
-                        obj_orientationW=msg.objects[i].pose.orientation.w
-                        objyaw=quaternion_to_euler(obj_orientationX,obj_orientationY,obj_orientationZ,obj_orientationW)
-                        obj_lable=msg.objects[i].label
-                        obj_dimX=msg.objects[i].dimensions.x
-                        obj_dimY=msg.objects[i].dimensions.y
-                        obj_dimZ=msg.objects[i].dimensions.z
-                        velocity_linear_x=msg.objects[i].velocity.linear.x
-                        velocity_linear_y=msg.objects[i].velocity.linear.y
-                        velocity_linear_z=msg.objects[i].velocity.linear.z
-                        
-                        velocity_angular_x=msg.objects[i].velocity.angular.x
-                        velocity_angular_y=msg.objects[i].velocity.angular.y
-                        velocity_angular_z=msg.objects[i].velocity.angular.z
-                        
-                        acceleration_linear_x=msg.objects[i].acceleration.linear.x
-                        acceleration_linear_y=msg.objects[i].acceleration.linear.y
-                        acceleration_linear_z=msg.objects[i].acceleration.linear.z
-                        
-                        acceleration_angular_x=msg.objects[i].acceleration.angular.x
-                        acceleration_angular_y=msg.objects[i].acceleration.angular.y
-                        acceleration_angular_z=msg.objects[i].acceleration.angular.z
-                        
-                        message_obj =[date_time,locationsimtime,Frame_robot_pose,obj_ID,objyaw,obj_x,obj_y,obj_z,obj_lable,obj_dimX,obj_dimY,obj_dimZ,
-                                      velocity_linear_x,velocity_linear_y,velocity_linear_z,velocity_angular_x,velocity_angular_y,
-                                      velocity_angular_z,acceleration_linear_x,acceleration_linear_y,acceleration_linear_z,
-                                      acceleration_angular_x,acceleration_angular_y,acceleration_angular_z]
-                        #print(f'{date_time},  {locationsimtime}')
+                        obj_orientationX = msg.objects[i].pose.orientation.x
+                        obj_orientationY = msg.objects[i].pose.orientation.y
+                        obj_orientationZ = msg.objects[i].pose.orientation.z
+                        obj_orientationW = msg.objects[i].pose.orientation.w
+                        objyaw = quaternion_to_euler(obj_orientationX, obj_orientationY, obj_orientationZ,
+                                                     obj_orientationW)
+                        obj_lable = msg.objects[i].label
+                        obj_dimX = msg.objects[i].dimensions.x
+                        obj_dimY = msg.objects[i].dimensions.y
+                        obj_dimZ = msg.objects[i].dimensions.z
+                        velocity_linear_x = msg.objects[i].velocity.linear.x
+                        velocity_linear_y = msg.objects[i].velocity.linear.y
+                        velocity_linear_z = msg.objects[i].velocity.linear.z
+
+                        velocity_angular_x = msg.objects[i].velocity.angular.x
+                        velocity_angular_y = msg.objects[i].velocity.angular.y
+                        velocity_angular_z = msg.objects[i].velocity.angular.z
+
+                        acceleration_linear_x = msg.objects[i].acceleration.linear.x
+                        acceleration_linear_y = msg.objects[i].acceleration.linear.y
+                        acceleration_linear_z = msg.objects[i].acceleration.linear.z
+
+                        acceleration_angular_x = msg.objects[i].acceleration.angular.x
+                        acceleration_angular_y = msg.objects[i].acceleration.angular.y
+                        acceleration_angular_z = msg.objects[i].acceleration.angular.z
+
+                        message_obj = [date_time, locationsimtime, Frame_robot_pose, obj_ID, objyaw, obj_x, obj_y,
+                                       obj_z, obj_lable, obj_dimX, obj_dimY, obj_dimZ,
+                                       velocity_linear_x, velocity_linear_y, velocity_linear_z, velocity_angular_x,
+                                       velocity_angular_y,
+                                       velocity_angular_z, acceleration_linear_x, acceleration_linear_y,
+                                       acceleration_linear_z,
+                                       acceleration_angular_x, acceleration_angular_y, acceleration_angular_z]
+                        # print(f'{date_time},  {locationsimtime}')
                         writer_objects.writerow(message_obj)
-                    #framenum_obj+=1 
+                    # framenum_obj+=1
             if topic == "/robot/final_trajectory":
-                PointNumber=1
+                PointNumber = 1
                 timestamp1 = rospy.Time.to_sec(t)
                 date_time1 = datetime.fromtimestamp(timestamp1)
-                locationsimtime1=(date_time1-first_message_time).total_seconds()
-                if ego_posx!=0 or ego_posy!=0:
+                locationsimtime1 = (date_time1 - first_message_time).total_seconds()
+                if ego_posx != 0 or ego_posy != 0:
                     for i in range(len(msg.waypoints)):
-                        Targetx=msg.waypoints[i].pose.pose.position.x
-                        Targety=msg.waypoints[i].pose.pose.position.y
-                        Targetz=msg.waypoints[i].pose.pose.position.z
-                        orientationX=msg.waypoints[i].pose.pose.orientation.x
-                        orientationY=msg.waypoints[i].pose.pose.orientation.y
-                        orientationZ=msg.waypoints[i].pose.pose.orientation.z
-                        orientationW=msg.waypoints[i].pose.pose.orientation.w
-                        TargetH=quaternion_to_euler(orientationX,orientationY,orientationZ,orientationW)
-                        message_trajectory =[date_time1,locationsimtime1,simFrame,PointNumber,ego_posx,ego_posy,poseZ,egoyaw,Targetx,Targety,Targetz,TargetH]
+                        Targetx = msg.waypoints[i].pose.pose.position.x
+                        Targety = msg.waypoints[i].pose.pose.position.y
+                        Targetz = msg.waypoints[i].pose.pose.position.z
+                        orientationX = msg.waypoints[i].pose.pose.orientation.x
+                        orientationY = msg.waypoints[i].pose.pose.orientation.y
+                        orientationZ = msg.waypoints[i].pose.pose.orientation.z
+                        orientationW = msg.waypoints[i].pose.pose.orientation.w
+                        TargetH = quaternion_to_euler(orientationX, orientationY, orientationZ, orientationW)
+                        message_trajectory = [date_time1, locationsimtime1, simFrame, PointNumber, ego_posx, ego_posy,
+                                              poseZ, egoyaw, Targetx, Targety, Targetz, TargetH]
                         writer_trajectory.writerow(message_trajectory)
-                        PointNumber+=1
-                    simFrame+=1
-                
+                        PointNumber += 1
+                    simFrame += 1
+
         robot_pos_file.close()
         objects_file.close()
         EgoState_file.close()
         trajectory_file.close()
-        
-        
 
+
+# if __name__ == '__main__':
 def parse(input_dir, output_dir):
-   #input_dir='/media/dell/HIKSEMI/pji_DGNC/pjioutrobot_2024-08-21-15-12-04.bag'
-   #output_dir='/media/dell/HIKSEMI/pji_DGNC'
-   # input_dir=sys.argv[1]
-   # output_dir = sys.argv[2]
-   bagname=input_dir.split('/')[-1].split('.')[0]
-
-   
-   output_dir=os.path.join(output_dir, bagname)
-   if not os.path.exists(output_dir):
-       os.makedirs(output_dir)
-   parsehancheng(input_dir, output_dir)
+    # input_dir='/media/dell/HIKSEMI/pji_DGNC/pjioutrobot_2024-08-21-15-12-04.bag'
+    # output_dir='/media/dell/HIKSEMI/pji_DGNC'
+    # input_dir=sys.argv[1]
+    # output_dir = sys.argv[2]
+    bagname = input_dir.split('/')[-1].split('.')[0]
 
+    output_dir = os.path.join(output_dir, bagname)
+    if not os.path.exists(output_dir):
+        os.makedirs(output_dir)
+    parsehancheng(input_dir, output_dir)

src/python2/pjibot_patrol/simulation-pjibot_partol.py

@@ -11,8 +11,6 @@ key1 = 'pjibot_patrol/'
 path1 = '/scenarios2/'
 path2 = '/mnt/disk001/dcl_data_process/src/python2/pjibot_patrol/'
 path3 = '/mnt/disk001/simulation_outdoor/'
-xodr = 'thq_1116.xodr'
-osgb = 'thq_1116.opt.osgb'
 vehicle_name = 'PuJin_patrol_robot'  # 配送 PuJin_distribution 巡检 PuJin_patrol_robot
 xoscName = 'scenario.xosc'
 
@@ -28,36 +26,6 @@ def move_xosc_before_simulation(root_path):
         tree1 = ET.parse(root_path + 'scenario_orig.xosc')
         root1 = tree1.getroot()
         for node0 in root1:
-            if node0.tag == 'RoadNetwork':
-                for node1 in node0:
-                    # 打印根元素的标签名
-                    src_file_prefix = path3
-                    if 'LogicFile' == node1.tag:
-                        if xodr in node1.get('filepath'):
-                            dst_file = root_path + xodr
-                            shutil.copy(src_file_prefix + xodr, dst_file)
-                            node1.set('filepath', dst_file)
-                        if xodr in node1.get('filepath'):
-                            dst_file = root_path + xodr
-                            shutil.copy(src_file_prefix + xodr, dst_file)
-                            node1.set('filepath', dst_file)
-                        if xodr in node1.get('filepath'):
-                            dst_file = root_path + xodr
-                            shutil.copy(src_file_prefix + xodr, dst_file)
-                            node1.set('filepath', dst_file)
-                    if 'SceneGraphFile' == node1.tag:
-                        if osgb in node1.get('filepath'):
-                            dst_file = root_path + osgb
-                            shutil.copy(src_file_prefix + osgb, dst_file)
-                            node1.set('filepath', dst_file)
-                        if osgb in node1.get('filepath'):
-                            dst_file = root_path + osgb
-                            shutil.copy(src_file_prefix + osgb, dst_file)
-                            node1.set('filepath', dst_file)
-                        if osgb in node1.get('filepath'):
-                            dst_file = root_path + osgb
-                            shutil.copy(src_file_prefix + osgb, dst_file)
-                            node1.set('filepath', dst_file)
             if node0.tag == 'Entities':
                 for node1 in node0:
                     if node1.get("name") == 'Ego':
@@ -113,15 +81,6 @@ def is_upload_completed(bucket, prefix):
     return int(count) == int(target_number)
 
 
-'''
-cname：http://open-bucket.oss.icvdc.com
-keyid：n8glvFGS25MrLY7j
-secret：xZ2Fozoarpfw0z28FUhtg8cu0yDc5d
-oss桶名： oss://open-bucket
-
-oss桶名： open-bucket
-内网endpoint: oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com
-'''
 # ------- 获取合并之后的bag包，解析出csv -------
 if __name__ == '__main__':
     # 1 创建阿里云对象
@@ -131,7 +90,6 @@ if __name__ == '__main__':
     while True:
         try:
             logging.info("开始新一轮扫描")
-            local_delete_list = []
             oss_delete_list = []
             prefix_list = []
             # 2 获取已经上传完成的所有目录并分组
@@ -165,18 +123,10 @@ if __name__ == '__main__':
                         if not os.path.exists(root_path1):
                             os.makedirs(root_path1)
                         bucket.get_object_to_file(parse_prefix_full + xoscName, root_path1 + 'scenario_orig.xosc')
-                        local_delete_list.append(root_path1 + 'scenario_orig.xosc')
                         move_xosc_before_simulation(root_path1)
                         simulation(parse_prefix_full, root_path1 + 'simulation.mp4')
                 except Exception as e:
                     logging.exception("局部异常处理: %s", str(e))
-            # 删除本地临时文件
-            if len(local_delete_list) > 0:
-                for local_delete in local_delete_list:
-                    try:
-                        os.remove(local_delete)
-                    except Exception as e:
-                        pass
             time.sleep(sleep_time)
         except Exception as e:
             logging.exception("全局异常处理: %s", str(e))

src/python2/pjisuv/callback-nohup.sh

@@ -5,4 +5,4 @@ if [ ! -d "./log" ]; then
 else
     echo "Directory './log' already exists."
 fi
-nohup python2 callback-pjisuv.py > log/callback.out 2>&1 &
+nohup python2 callback-pjisuv.py > log/callback-pjisuv.out 2>&1 &

src/python2/pjisuv/callback-pjisuv.py

@@ -6,17 +6,59 @@ import oss2
 from datetime import datetime, timedelta
 import logging
 
-path1 = '/mnt/disk001/cicv-data-closedloop/python2-pjisuv-module/'
+path1 = '/mnt/disk001/dcl_data_process/src/python2/pjisuv/'
 
-logging.basicConfig(filename=path1 + 'log/callback.log', level=logging.INFO,
+logging.basicConfig(filename=path1 + 'log/callback-pjisuv.log', level=logging.INFO,
                     format='%(asctime)s - %(levelname)s - %(message)s')
 
 key1 = 'pjisuv/'
-sleep_time = 60  # 每多少秒扫描一次
+sleep_time = 30  # 每多少秒扫描一次
 
 url1_private = "http://10.14.86.127:9081/device/auth"
 url2_private = "http://10.14.86.127:9081/device/data/callback"
 
+error_bag_json = "/mnt/disk001/dcl_data_process/src/python2/pjisuv/callback-errorBag.json"
+
+
+def parse_json_to_string_array(file_path):
+    try:
+        # 打开并读取JSON文件（Python 2中不支持encoding参数，需要使用codecs模块或处理文件读取后的编码）
+        with open(file_path, 'r') as file:
+            # 读取文件内容
+            file_content = file.read()
+            # 解析JSON内容（Python 2中json.loads用于解析字符串）
+            data = json.loads(file_content.decode('utf-8'))  # 假设文件是UTF-8编码，这里需要手动解码
+
+        # 检查数据是否是一个列表，并且列表中的元素是否是字符串
+        if isinstance(data, list):
+            for item in data:
+                if not isinstance(item, basestring):  # Python 2中字符串类型包括str和unicode，用basestring检查
+                    raise ValueError("JSON数组中的元素不是字符串")
+            return data
+        else:
+            return []
+    except Exception as e:
+        return []
+
+
+def list_to_json_file(data, file_path):
+    """
+    将列表转换为JSON格式并写入指定的文件路径。
+    如果文件已存在，则覆盖它。
+
+    参数:
+    data (list): 要转换为JSON的列表。
+    file_path (str): 要写入JSON数据的文件路径。
+    """
+    # 将列表转换为JSON格式的字符串，并确保输出为UTF-8编码的字符串
+    json_data = json.dumps(data, ensure_ascii=False, indent=4)
+    json_data_utf8 = json_data.encode('utf-8')  # 编码为UTF-8
+
+    # 以写入模式打开文件，如果文件已存在则覆盖
+    with open(file_path, 'w') as file:
+        # 将UTF-8编码的JSON字符串写入文件
+        file.write(json_data_utf8)
+
 
 def add_hour(date_string, hour_number):
     original_date = datetime.strptime(date_string, "%Y-%m-%d-%H-%M-%S")
@@ -61,7 +103,7 @@ if __name__ == '__main__':
                 plan_bag = False
                 control_bag = False
                 callback_json = False
-                for obj2 in oss2.ObjectIterator(bucket, prefix=prefix+ '/'):
+                for obj2 in oss2.ObjectIterator(bucket, prefix=prefix + '/'):
                     if 'callback.json' in str(obj2.key):
                         callback_json = True
                     if 'camera.mp4' in str(obj2.key):
@@ -81,6 +123,9 @@ if __name__ == '__main__':
 
                 if not callback_json or not camera_mp4 or not drive_csv or not pcd_forwardlook_mp4 or not pcd_overlook_mp4 or not pos_orig_csv or not scenario_orig_mp4 or not scenario_orig_xosc:
                     continue
+                error_bag_list = parse_json_to_string_array(error_bag_json)
+                if str(obj1.key) in error_bag_list:
+                    continue
                 time.sleep(2)
                 logging.info("发送： %s" % str(obj1.key))
                 # 1 获取json内容
@@ -92,18 +137,15 @@ if __name__ == '__main__':
                     "secretKey": json_object['secretKey']
                 }
                 json_data1 = json.dumps(data1)
-                logging.info("授权接口请求中： %s" % url1_private)
                 request1 = urllib2.Request(url1_private, json_data1,
                                            headers={'Content-Type': 'application/json'})
                 response1 = urllib2.urlopen(request1)
                 result_json1 = response1.read()
                 result_object1 = json.loads(result_json1)
-                logging.info("授权接口请求结果为： %s", result_object1)
                 try:
                     access_token = result_object1.get('data').get('accessToken')
                     logging.info("bag文件为：%s" % str(json_object['rosBagPath']))
                     old_date = json_object['dataName']
-                    data_size = bucket.get_object_meta(json_object['rosBagPath']).content_length
                     equipment_no = json_object['equipmentNo']
                     old_file_path = json_object['filePath']
                     old_ros_bag_path = json_object['rosBagPath']
@@ -113,32 +155,59 @@ if __name__ == '__main__':
                     logging.exception("callback报错： %s" % str(e))
                     continue
 
-                # 将时区统一
-                new_date = add_hour(old_date, 8)
+                upload = False
+                if 'userId' in json_object:
+                    logging.info("手动上传的数据")
+                    upload = True
+                    old_date = ''
+                else:
+                    logging.info("自动采集的数据")
+                    upload = False
+
                 old_delete_list = []
+                old_delete_callback = ''
+                new_date = ''
+                # 复制 data_parse
                 for obj_old in oss2.ObjectIterator(bucket, prefix=old_file_path):
                     old_delete_list.append(str(obj_old.key))
                     if 'callback.json' in str(obj_old.key):
-                        if old_date is None:
-                            bucket.copy_object(bucket_name, str(obj_old.key),
-                                               str(obj_old.key).replace('callback.json',
-                                                                        'callback_done.json'))
-                        else:
+                        if not upload:
+                            new_date = add_hour(old_date, 8)
+                            # 将时区统一
                             bucket.copy_object(bucket_name, str(obj_old.key),
                                                str(obj_old.key).replace(old_date, new_date).replace('callback.json',
                                                                                                     'callback_done.json'))
+                        else:
+                            old_delete_callback = str(obj_old.key)
+                            bucket.copy_object(bucket_name, str(obj_old.key),
+                                               str(obj_old.key).replace('callback.json',
+                                                                        'callback_done.json'))
+
                     else:
-                        bucket.copy_object(bucket_name, str(obj_old.key), str(obj_old.key).replace(old_date, new_date))
-                bucket.copy_object(bucket_name, old_ros_bag_path, old_ros_bag_path.replace(old_date, new_date))
-                bucket.delete_object(old_ros_bag_path)
-                bucket.batch_delete_objects(old_delete_list)
+                        if not upload:
+                            new_date = add_hour(old_date, 8)
+                            bucket.copy_object(bucket_name, str(obj_old.key),
+                                               str(obj_old.key).replace(old_date, new_date))
 
-                if 'userId' in json_object:
-                    logging.info("json_object 包含 'userId' 字段，值为:", json_object['userId'])
+                # 处理是否上传
+                if not upload:
+                    try:
+                        bucket.copy_object(bucket_name, old_ros_bag_path, old_ros_bag_path.replace(old_date, new_date))
+                        bucket.delete_object(old_ros_bag_path)
+                        bucket.batch_delete_objects(old_delete_list)
+                    except Exception as e:
+                        error_bag_list = parse_json_to_string_array(error_bag_json)
+                        error_bag_list.append(str(obj1.key))
+                        list_to_json_file(error_bag_list, error_bag_json)
+                        continue
+                else:
+                    bucket.delete_object(old_delete_callback)
+
+                if upload:
                     data2 = {
                         'userId': json_object['userId'],
-                        "dataName": new_date,
-                        "dataSize": data_size,
+                        "dataName": json_object['dataName'],
+                        "dataSize": json_object['dataSize'],
                         "equipmentNo": equipment_no,
                         "filePath": old_file_path.replace(old_date, new_date),
                         "rosBagPath": old_ros_bag_path.replace(old_date, new_date),
@@ -146,10 +215,9 @@ if __name__ == '__main__':
                         "triggerId": trigger_id
                     }
                 else:
-                    logging.info("json_object 不包含 'userId' 字段")
                     data2 = {
                         "dataName": new_date,
-                        "dataSize": data_size,
+                        "dataSize": bucket.get_object_meta(json_object['rosBagPath']).content_length,
                         "equipmentNo": equipment_no,
                         "filePath": old_file_path.replace(old_date, new_date),
                         "rosBagPath": old_ros_bag_path.replace(old_date, new_date),

src/python2/pjisuv/camera-pjisuv.py

@@ -87,7 +87,6 @@ if __name__ == '__main__':
                     oss_delete_list = []
                     # 获取合并后的包
                     merged_bag_object_key = str(obj1.key)
-                    # print(f'判断1{merged_bag_object_key}')
                     if 'data_merge' in str(obj1.key) and str(obj1.key).endswith('.bag'):
                         merged_bag_object_key_split = merged_bag_object_key.split('/')
                         merged_prefix = '/'.join(merged_bag_object_key_split[:-1])

src/python2/pjisuv/pcd-pjisuv.py

@@ -8,7 +8,7 @@ import logging
 
 path1 = '/mnt/disk001/dcl_data_process/src/python2/pjisuv/'
 
-logging.basicConfig(filename=path1 + 'log/pcd_pjisuv.log', level=logging.INFO,
+logging.basicConfig(filename=path1 + 'log/pcd-pjisuv.log', level=logging.INFO,
                     format='%(asctime)s - %(levelname)s - %(message)s')
 
 key1 = 'pjisuv/'

src/python2/pjisuv/simulation-pjisuv.py

@@ -8,11 +8,11 @@ import docker
 import logging
 
 key1 = 'pjisuv/'
-path1 = '/scenarios2/'
+path1 = '/scenarios3/'
 path2 = '/mnt/disk001/dcl_data_process/src/python2/pjisuv/'
 path3 = '/mnt/disk001/simulation_pjisuv/'
 
-logging.basicConfig(filename=path2 + 'log/simulation.log', level=logging.INFO,
+logging.basicConfig(filename=path2 + 'log/simulation-pjisuv.log', level=logging.INFO,
                     format='%(asctime)s - %(levelname)s - %(message)s')
 
 sleep_time = 60  # 每多少秒扫描一次
@@ -75,14 +75,14 @@ def upload_simulation(parse_prefix, mp41):
         logging.info('上传仿真视频到 %s' % parse_prefix + 'scenario_orig.mp4')
         # bucket.put_object_from_file(parse_prefix + 'scenario_hmi.mp4', mp42)
         # logging.info('上传仿真视频到 %s' % parse_prefix + 'scenario_hmi.mp4')
-        shutil.rmtree(path1) # 仿真完就删除
+        shutil.rmtree(path1)  # todo 仿真完就删除了，所以不需要 local_delete_list
     except Exception as e:
         logging.exception("上传视频报错 %s" % str(e))
 
 
 def simulation(parse_prefix, mp41):
     try:
-        os.system("docker start vtd2")
+        os.system("docker start vtd3")
         # 实例化Docker客户端
         client = docker.from_env()
         while True:
@@ -92,7 +92,7 @@ def simulation(parse_prefix, mp41):
             run = False
             # 打印容器列表
             for container in containers:
-                if 'vtd2' == container.name:
+                if 'vtd3' == container.name:
                     run = True
                     break
             if not run:
@@ -112,26 +112,14 @@ def is_upload_completed(bucket, prefix):
     return int(count) == int(target_number)
 
 
-'''
-cname：http://open-bucket.oss.icvdc.com
-keyid：n8glvFGS25MrLY7j
-secret：xZ2Fozoarpfw0z28FUhtg8cu0yDc5d
-oss桶名： oss://open-bucket
-
-oss桶名： open-bucket
-内网endpoint: oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com
-'''
 # ------- 获取合并之后的bag包，解析出csv -------
 if __name__ == '__main__':
     # 1 创建阿里云对象
     auth = oss2.Auth('n8glvFGS25MrLY7j', 'xZ2Fozoarpfw0z28FUhtg8cu0yDc5d')
-    # cname = 'http://open-bucket.oss.icvdc.com'
-    # bucket = oss2.Bucket(auth, cname, 'open-bucket', is_cname=True)
     endpoint = 'oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com'
     bucket = oss2.Bucket(auth, endpoint, 'open-bucket')
     while True:
         logging.info("开始新一轮扫描")
-        local_delete_list = []
         oss_delete_list = []
         prefix_list = []
         # 2 获取已经上传完成的所有目录并分组
@@ -162,15 +150,6 @@ if __name__ == '__main__':
                 os.makedirs(root_path1)
 
             bucket.get_object_to_file(parse_prefix_full + 'scenario_orig.xosc', root_path1 + 'scenario_orig.xosc')
-            local_delete_list.append(root_path1 + 'scenario_orig.xosc')
             move_xosc_before_simulation(root_path1)
             simulation(parse_prefix_full, root_path1 + 'simulation.mp4')
-
-        # 删除本地临时文件
-        if len(local_delete_list) > 0:
-            for local_delete in local_delete_list:
-                try:
-                    os.remove(local_delete)
-                except Exception as e:
-                    logging.exception("删除本地临时文件报错: %s" % str(e))
         time.sleep(sleep_time)

src/python2/pjisuv/simulation-tail.sh

@@ -1,2 +0,0 @@
-#!/bin/bash
-tail -f log/simulation.log

src/python2/pjisuv/xosc-pjisuv.py

@@ -3,16 +3,59 @@ import os
 import time
 import oss2
 import logging
+import json
 
 path1 = '/mnt/disk001/dcl_data_process/src/python2/pjisuv/'
 path2 = '/mnt/disk001/dcl_data_process/src/python3/pjisuv/'
 
-logging.basicConfig(filename=path1 + 'log/xosc.log', level=logging.INFO,
+logging.basicConfig(filename=path1 + 'log/xosc-pjisuv.log', level=logging.INFO,
                     format='%(asctime)s - %(levelname)s - %(message)s')
 
 key1 = 'pjisuv/'
 sleep_time = 60  # 每多少秒扫描一次
 
+error_bag_json = "/mnt/disk001/dcl_data_process/src/python2/pjisuv/xosc-errorBag.json"
+
+
+def parse_json_to_string_array(file_path):
+    try:
+        # 打开并读取JSON文件（Python 2中不支持encoding参数，需要使用codecs模块或处理文件读取后的编码）
+        with open(file_path, 'r') as file:
+            # 读取文件内容
+            file_content = file.read()
+            # 解析JSON内容（Python 2中json.loads用于解析字符串）
+            data = json.loads(file_content.decode('utf-8'))  # 假设文件是UTF-8编码，这里需要手动解码
+
+        # 检查数据是否是一个列表，并且列表中的元素是否是字符串
+        if isinstance(data, list):
+            for item in data:
+                if not isinstance(item, basestring):  # Python 2中字符串类型包括str和unicode，用basestring检查
+                    raise ValueError("JSON数组中的元素不是字符串")
+            return data
+        else:
+            return []
+    except Exception as e:
+        return []
+
+
+def list_to_json_file(data, file_path):
+    """
+    将列表转换为JSON格式并写入指定的文件路径。
+    如果文件已存在，则覆盖它。
+
+    参数:
+    data (list): 要转换为JSON的列表。
+    file_path (str): 要写入JSON数据的文件路径。
+    """
+    # 将列表转换为JSON格式的字符串，并确保输出为UTF-8编码的字符串
+    json_data = json.dumps(data, ensure_ascii=False, indent=4)
+    json_data_utf8 = json_data.encode('utf-8')  # 编码为UTF-8
+
+    # 以写入模式打开文件，如果文件已存在则覆盖
+    with open(file_path, 'w') as file:
+        # 将UTF-8编码的JSON字符串写入文件
+        file.write(json_data_utf8)
+
 
 def generate_xosc(parse_prefix, local_parse_dir, local_delete_list):
     try:
@@ -23,26 +66,21 @@ def generate_xosc(parse_prefix, local_parse_dir, local_delete_list):
         os.system(command2)
         local_xosc_path2 = local_parse_dir + 'simulation_orig/xosc/openx0.xosc'
         bucket.put_object_from_file(parse_prefix + 'scenario_orig.xosc', local_xosc_path2)
-        bucket.put_object_from_file(parse_prefix + 'scenario_hmi.xosc', local_xosc_path2) # todo 将orig也上传成hmi，因为多功能车暂时没有hmi
+        bucket.put_object_from_file(parse_prefix + 'scenario_hmi.xosc',
+                                    local_xosc_path2)  # todo 将orig也上传成hmi，因为多功能车暂时没有hmi
         logging.info("上传 scenario_orig.xosc 成功: %s" % str(parse_prefix + 'scenario_orig.xosc'))
         local_delete_list.append(local_xosc_path2)
     except Exception as e:
-        logging.exception("生成xosc报错: %s" % str(e))
+        error_bag_list = parse_json_to_string_array(error_bag_json)
+        error_bag_list.append(parse_prefix)
+        list_to_json_file(error_bag_list, error_bag_json)
+        logging.exception(parse_prefix, "生成 xosc 报错，将其添加到错误bag列表。", str(e))
 
 
-'''
-cname：http://open-bucket.oss.icvdc.com
-内网endpoint: oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com
-oss桶名： open-bucket
-keyid：n8glvFGS25MrLY7j
-secret：xZ2Fozoarpfw0z28FUhtg8cu0yDc5d
-'''
 # ------- 获取合并之后的bag包，解析出csv -------
 if __name__ == '__main__':
     # 1 创建阿里云对象
     auth = oss2.Auth('n8glvFGS25MrLY7j', 'xZ2Fozoarpfw0z28FUhtg8cu0yDc5d')
-    # cname = 'http://open-bucket.oss.icvdc.com'
-    # bucket = oss2.Bucket(auth, cname, 'open-bucket', is_cname=True)
     endpoint = 'oss-cn-beijing-gqzl-d01-a.ops.gqzl-cloud.com'
     bucket = oss2.Bucket(auth, endpoint, 'open-bucket')
     while True:
@@ -74,17 +112,20 @@ if __name__ == '__main__':
                         #     csv2_done = True
                     if xosc1_done or not csv1_done:
                         continue
+                    error_bag_list = parse_json_to_string_array(error_bag_json)
+                    if str(obj2_key) in error_bag_list:
+                        continue
+
                     logging.info("需要生成xosc: %s" % obj2_key)
-                    parse_prefix_full = obj2_key
-                    local_dir_full = path1 + parse_prefix_full
+                    local_dir_full = path1 + obj2_key
                     if not os.path.exists(local_dir_full):
                         os.makedirs(local_dir_full)
                     # todo 下载单个csv生成两个视频？
-                    bucket.get_object_to_file(parse_prefix_full + 'pos_orig.csv', local_dir_full + 'pos_orig.csv')
-                    # bucket.get_object_to_file(parse_prefix_full+'pos_hmi.csv', local_dir_full+'pos_hmi.csv')
+                    bucket.get_object_to_file(obj2_key + 'pos_orig.csv', local_dir_full + 'pos_orig.csv')
+                    # bucket.get_object_to_file(obj2_key+'pos_hmi.csv', local_dir_full+'pos_hmi.csv')
                     # local_delete_list.append(local_dir_full + 'pos_orig.csv') # todo 不删除了 做测试用
                     # local_delete_list.append(local_dir_full+'pos_hmi.csv')
-                    generate_xosc(parse_prefix_full, local_dir_full, local_delete_list)
+                    generate_xosc(obj2_key, local_dir_full, local_delete_list)
 
                     # 删除本地临时文件
                     if len(local_delete_list) > 0:

src/python3/pjibot_outdoor/jiqiren_outdoor.py

@@ -50,7 +50,6 @@ class Batchrun:
         return FileList
 
     # 自定义一个函数来过滤行
-
     def generateScenarios_raw(self, absPath, vehicle_type):
         '''
         原始自然驾驶场景还原
@@ -71,16 +70,37 @@ class Batchrun:
         posdata_ego.rename(columns={'X': 'pos_x'}, inplace=True)
         posdata_ego.rename(columns={'Y': 'pos_y'}, inplace=True)
 
-        def trans_pos(posdata):
-            theta = 1.4
-            input_lat1 = 39.7286896
-            input_lon1 = 116.4885025
+        def get_map_id(df):
+            # 太和桥园区经纬度
+            taiheqiao_latitude = 39.7286896
+            taiheqiao_longitude = 116.4885025
+            # 安庆经纬度
+            anqing_latitude = 30.5585070
+            anqing_longitude = 117.1850840
+
+            points = [(taiheqiao_latitude, taiheqiao_longitude), (anqing_latitude, anqing_longitude)]
+            cur_latitude = df.iloc[0]['latitude']
+            cur_longitude = df.iloc[0]['longitude']
+
+            # Calculate distances to each point
+            distances = []
+            for lat, lon in points:
+                distance = np.sqrt((lon - cur_longitude) ** 2 + (lat - cur_latitude) ** 2)
+                distances.append(distance)
+
+            # Find the index of the nearest point
+            nearest_index = np.argmin(distances)
+
+            return nearest_index
+
+        def trans_pos(posdata, map_id):
+            theta_dict = {0:1.4, 1: 1.055}
+            trans_input = {0: (39.7286896, 116.4885025), 1: (30.5585070, 117.1850840)}
+            trans_utm = {0: (456256.260152, 4397809.886833), 1: (517696.8132810, 3380770.294203)}
             transformer = Transformer.from_crs("epsg:4326", "epsg:32650")
-            utm50n_x2 = 456256.260152
-            utm50n_y2 = 4397809.886833
-            utm50n_x1, utm50n_y1 = transformer.transform(input_lat1, input_lon1)
-            translation = np.array([utm50n_x1 - utm50n_x2, utm50n_y1 - utm50n_y2])
-            rotation_matrix = R.from_euler('z', theta).as_matrix()
+            utm50n_x1, utm50n_y1 = transformer.transform(trans_input[map_id][0], trans_input[map_id][1])
+            translation = np.array([utm50n_x1 - trans_utm[map_id][0], utm50n_y1 - trans_utm[map_id][1]])
+            rotation_matrix = R.from_euler('z', theta_dict[map_id]).as_matrix()
             # 提取原始坐标
             original_positions = posdata[['pos_x', 'pos_y']].values
             # 应用旋转和平移
@@ -89,11 +109,12 @@ class Batchrun:
             # 更新 posdata
             posdata['pos_x'] = rotated_positions[:, 0]
             posdata['pos_y'] = rotated_positions[:, 1]
-            posdata['HeadingAngle'] += theta
+            posdata['HeadingAngle'] += theta_dict[map_id]
             return posdata
 
-        posdata_obs = trans_pos(posdata_obs)
-        posdata_ego = trans_pos(posdata_ego)
+        map_id = get_map_id(posdata_ego)
+        posdata_obs = trans_pos(posdata_obs, map_id)
+        posdata_ego = trans_pos(posdata_ego, map_id)
         posdata_obs['simtime'] = posdata_obs['simtime'].round(1)
         posdata_ego['simtime'] = posdata_ego['simtime'].round(1)
 
@@ -183,7 +204,8 @@ class Batchrun:
         posdata = pd.concat([posdata_obs, posdata_ego], ignore_index=True)
         posdata = posdata.sort_values(by='simtime')
 
-        posdata['altitude'] = 19.27
+        altitude_dict = {0: 19.27, 1: 8.48332}
+        posdata['altitude'] = altitude_dict[map_id]
         posdata['AbsVel'] = 0
 
         pos_ego = posdata.loc[posdata['ID'] == -1, ['simtime', 'pos_x', 'pos_y', 'HeadingAngle', 'altitude', 'Type']]
@@ -263,7 +285,6 @@ class Batchrun:
         #
         #     return group
 
-
         def calculate_heading_angle(group):
             start_idx = 0
             length = len(group)
@@ -368,7 +389,7 @@ class Batchrun:
         # if time_of_day >= 64800:
         #     time_of_day = 64800
 
-        s = Scenario(ego_points, object_points, gps_time, ego_speed, work_mode, period, absPath, 0, vehicle_type)
+        s = Scenario(ego_points, object_points, gps_time, ego_speed, work_mode, period, absPath, 0, vehicle_type, map_id)
         filename = absPath + '/simulation'
         print(filename)
         files = s.generate(filename)
@@ -443,7 +464,7 @@ class Batchrun:
 
 
 if __name__ == "__main__":
-    # rootPath = "/media/hancheng/Simulation5/pujin/pujin_outdoor/10_23"  # 跟车
+    # rootPath = "/media/hancheng/Simulation5/anqing/11_5"  # 跟车
     rootPath = sys.argv[1]
     vehicle_type = sys.argv[2]
     # vehicle_type = "0"

src/python3/pjibot_outdoor/openx_outdoor.py

@@ -11,6 +11,7 @@ from scenariogeneration import xosc
 from scenariogeneration import ScenarioGenerator
 from scenariogeneration.xodr import RoadSide, Object, ObjectType, Dynamic, Orientation
 import math, os
+# from xodr_generator2 import road_detector_new
 from datetime import datetime
 import traceback
 import json
@@ -21,11 +22,11 @@ from xml.sax.saxutils import escape
 import warnings
 warnings.filterwarnings("ignore")
 
-xodr_list = ['/home/hancheng/taiheqiao_map/thq_1116.xodr', '/home/hancheng/gongyuanbeihuan_map/OD/OpenDRIVE.xodr','/home/hancheng/glyt_map/OpenDRIVE/OpenDRIVE2.xodr']
-osgb_list = ['/home/hancheng/taiheqiao_map/thq_1116.opt.osgb', '/home/hancheng/gongyuanbeihuan_map/osgb/GJBM_20km_0822.opt.osgb', '/home/hancheng/glyt_map/OSGB/jinlong_20231129_2_guazai.opt.osgb']
+xodr_list = ['/mnt/disk001/simulation_outdoor/thq_1116.xodr', '/mnt/disk001/simulation_outdoor/anqing.xodr']
+osgb_list = ['/mnt/disk001/simulation_outdoor/thq_1116.opt.osgb', '/mnt/disk001/simulation_outdoor/anqing.osgb']
 
 class Scenario(ScenarioGenerator):
-    def __init__(self, gps, obs, gps_time, ego_speed, work_mode, period, abspath, timeofday, vehicle_type):
+    def __init__(self, gps, obs, gps_time, ego_speed, work_mode, period, abspath, timeofday, vehicle_type, map_id):
         ScenarioGenerator.__init__(self)
         self.gps = gps
         self.obs = obs
@@ -38,6 +39,7 @@ class Scenario(ScenarioGenerator):
         self.visual_fog_range = 20000  # 正常的能见度，没有雾
         self.time = 43200  # 强制为白天
         self.vehicle_type = vehicle_type
+        self.map_id = map_id
         # self.map_id = map_id
         # self.ObjectID = ObjectID
         # self.Speed = Speed
@@ -213,8 +215,7 @@ class Scenario(ScenarioGenerator):
                 
                 print(laneDF[laneDF['FrameID']>1860][['FrameID', 'LaneID', 'LanePosition']].head(50))
         print('road_label: ', road_label)
-        
-        
+
         return laneDF
         
     # def road_new(self, **kwargs):
@@ -338,8 +339,7 @@ class Scenario(ScenarioGenerator):
         
         lasth = float(egodata.at[0, h])
         init_flag = True
-    
-        # two method to set z value
+
         for row in egodata.iterrows():
             hhh = math.radians(row[1][h])
             if init_flag:
@@ -358,8 +358,8 @@ class Scenario(ScenarioGenerator):
         return position, time, ego_type
 
     def scenario(self, **kwargs):
-        road = xosc.RoadNetwork(roadfile='/home/hancheng/maps/taiheqiao_map/thq_1116.xodr',
-                                scenegraph='/home/hancheng/maps/taiheqiao_map/thq_1116.opt.osgb')
+        road = xosc.RoadNetwork(roadfile=xodr_list[self.map_id],
+                                scenegraph=osgb_list[self.map_id])
         catalog = xosc.Catalog()
         catalog.add_catalog('VehicleCatalog', 'Distros/Current/Config/Players/Vehicles')
         catalog.add_catalog('PedestrianCatalog', 'Distros/Current/Config/Players/Pedestrians')
@@ -620,7 +620,7 @@ class Scenario(ScenarioGenerator):
 
         sce = xosc.Scenario('my scenario', 'Maggie', paramet, entities, sb, road, catalog)
         return sce
-    
+
     def createUDAction(self, name):
         # tree = ET.parse('./models/ped_CDATA.xosc')
         # root = tree.getroot()
@@ -640,12 +640,14 @@ class Scenario(ScenarioGenerator):
         newnode.text = "<![CDATA[\n{}]\n]>".format(text)
         return newnode
 
+
 class WorkMode(Enum):
     cicv = 0  # CICV车端
     roadside = 1  # 车网路端
     car = 2  # 车网车端
     merge = 3  # 车网车端路端融合
-    
+
+
 def get_obj_type(mode):
     if mode == WorkMode.cicv.value:
         # CICV车端
@@ -681,6 +683,7 @@ def get_obj_type(mode):
         cone_type = [103]
     return ped_type, car_type, bicycle_motor_type, bus_type, truck_type, cone_type
 
+
 # 创建道路旁静止的场景
 def create_static_object(road, object_dict):
     for single_object in object_dict: