commit

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':

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':

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':

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,21 +11,23 @@ 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
 import pandas as pd
 from enum import Enum
+import elevation_0323 as el
 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 +40,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 +216,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):
@@ -340,26 +342,52 @@ class Scenario(ScenarioGenerator):
         init_flag = True
     
         # two method to set z value
-        for row in egodata.iterrows():
-            hhh = math.radians(row[1][h])
-            if init_flag:
-                position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=float(row[1][z]), h=hhh, p=0, r=0))
-                init_flag = False
-            else:
-                if float(row[1][h]) - lasth > 300:
-                    hhh = math.radians(float(row[1][h]) - 360)
-                elif float(row[1][h]) - lasth < -300:
-                    hhh = math.radians(float(row[1][h]) + 360)
-                position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=float(row[1][z]), h=hhh, p=0, r=0))
-                lasth = float(row[1][h])
-            time.append(float(row[1][t]))
-            # time.append(cur_time)
-            # cur_time += 0.1
-        return position, time, ego_type
+        zflag = 0 # 1
+        if zflag:
+            # 根据地图的xy值计算z值
+            zfile = '/home/hancheng/weihao/gongyuanbei_elevation.csv'
+            # zfile = '/home/hancheng/simulation_dynamic/elevation_df_taiheqiao.csv'
+            zdata = pd.read_csv(zfile)
+
+            for row in egodata.iterrows():
+                hhh = math.radians(row[1][h])
+                xodr_z = el.cal_elevation(zdata, float(row[1][e]), float(row[1][n]))
+                if init_flag:
+                    position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=xodr_z, h=hhh, p=0, r=0))
+                    init_flag = False
+                else:
+                    if float(row[1][h]) - lasth > 300:
+                        hhh = math.radians(float(row[1][h]) - 360)
+                    elif float(row[1][h]) - lasth < -300:
+                        hhh = math.radians(float(row[1][h]) + 360)
+                    position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=xodr_z, h=hhh, p=0, r=0))
+                    lasth = float(row[1][h])
+                time.append(float(row[1][t]))
+                # time.append(cur_time)
+                # cur_time += 0.1
+            return position, time, ego_type
+        else:
+            # z = 0
+            for row in egodata.iterrows():
+                hhh = math.radians(row[1][h])
+                if init_flag:
+                    position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=float(row[1][z]), h=hhh, p=0, r=0))
+                    init_flag = False
+                else:
+                    if float(row[1][h]) - lasth > 300:
+                        hhh = math.radians(float(row[1][h]) - 360)
+                    elif float(row[1][h]) - lasth < -300:
+                        hhh = math.radians(float(row[1][h]) + 360)
+                    position.append(xosc.WorldPosition(x=float(row[1][e]), y=float(row[1][n]), z=float(row[1][z]), h=hhh, p=0, r=0))
+                    lasth = float(row[1][h])
+                time.append(float(row[1][t]))
+                # time.append(cur_time)
+                # cur_time += 0.1
+            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 +648,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 +668,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 +711,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: