commit

src/python3/pjibot_outdoor/jiqiren_outdoor.py

@@ -5,6 +5,9 @@ import pandas as pd
 import subprocess
 import time
 import multiprocessing
+from pyproj import CRS, Transformer
+from scipy.spatial.transform import Rotation as R
+from scipy.signal import savgol_filter
 import xml.etree.ElementTree as ET
 import sys
 # 导入进程模块
@@ -68,6 +71,32 @@ 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
+            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()
+            # 提取原始坐标
+            original_positions = posdata[['pos_x', 'pos_y']].values
+            # 应用旋转和平移
+            rotated_positions =original_positions @ rotation_matrix[:2,:2].T # 只应用 2D 旋转
+            rotated_positions += translation
+            # 更新 posdata
+            posdata['pos_x'] = rotated_positions[:, 0]
+            posdata['pos_y'] = rotated_positions[:, 1]
+            posdata['HeadingAngle'] += theta
+            return posdata
+
+        posdata_obs = trans_pos(posdata_obs)
+        posdata_ego = trans_pos(posdata_ego)
+        posdata_obs['simtime'] = posdata_obs['simtime'].round(1)
+        posdata_ego['simtime'] = posdata_ego['simtime'].round(1)
+
         # 对障碍物数据进行处理
         def filter_rows(group):
             # 首先对x进行排序
@@ -78,6 +107,21 @@ class Batchrun:
             group = group[diffs >= 0.2]
             return group
 
+        def interpolate_missing_values(df):
+            # 1. 生成一个完整的以0.1秒为间隔的simtime
+            new_simtime = np.arange(df['simtime'].min(), df['simtime'].max() + 0.1, 0.1)
+
+            # 2. 创建新的DataFrame，并与原始DataFrame对齐
+            new_df = pd.DataFrame(new_simtime, columns=['simtime'])
+
+            # 3. 将原始DataFrame与新simtime进行合并
+            new_df = pd.merge(new_df, df, on='simtime', how='left')
+
+            # 4. 对列进行插值（线性插值）
+            new_df = new_df.interpolate(method='linear')
+
+            return new_df
+
         posdata_obs = posdata_obs.groupby(['simtime', 'pos_y'], group_keys=False).apply(filter_rows)
         posdata_obs.reset_index(drop=True, inplace=True)
 
@@ -85,6 +129,7 @@ class Batchrun:
         # posdata_ego['Time'] = posdata_ego['Time'] - start_time_ego
         # posdata_ego['Time'] = (posdata_ego['Time'] - (posdata_ego['Time'] % 100)) / 1000
         posdata_ego = posdata_ego.drop_duplicates(subset='simtime', keep='first')
+        posdata_ego = interpolate_missing_values(posdata_ego)
         posdata_ego['Type'] = 2
         posdata_ego['ID'] = -1
         # posdata_ego['pos_x'] = posdata_ego['pos_x'] - 88.96626
@@ -159,6 +204,66 @@ class Batchrun:
             pos_obs['ID'] != -1, ['simtime', 'ID', 'pos_x', 'pos_y', 'HeadingAngle', 'AbsVel', 'altitude', 'Type']]
         pos_obs = pos_obs.reset_index(drop=True)
 
+        # def calculate_heading_angle(group):
+        #     start_idx = 0
+        #     length = len(group)
+        #     prev_heading_angle = None  # 用于存储上一个计算的航向角
+        #
+        #     while start_idx < length:
+        #         start_pos_x, start_pos_y = group.iloc[start_idx]['pos_x'], group.iloc[start_idx]['pos_y']
+        #
+        #         # 找到与当前帧距离超过1米的帧
+        #         for i in range(start_idx + 1, length):
+        #             current_pos_x, current_pos_y = group.iloc[i]['pos_x'], group.iloc[i]['pos_y']
+        #             distance = np.sqrt((current_pos_x - start_pos_x) ** 2 + (current_pos_y - start_pos_y) ** 2)
+        #
+        #             if distance >= 1:
+        #                 # 计算航向角
+        #                 heading_angle = np.degrees(np.arctan2(current_pos_y - start_pos_y, current_pos_x - start_pos_x))
+        #
+        #                 # 检查航向角与前一个航向角的差值
+        #                 if prev_heading_angle is not None:
+        #                     angle_diff = np.abs(heading_angle - prev_heading_angle)
+        #                     if angle_diff > 180:
+        #                         angle_diff = 360 - angle_diff
+        #
+        #                     if angle_diff > 20:
+        #                         # 如果航向角差值大于25度，跳过当前点
+        #                         continue
+        #
+        #                 # 将 start_idx 到 i 之间的航向角设置为 heading_angle
+        #                 group.iloc[start_idx:i, group.columns.get_loc('HeadingAngle')] = heading_angle
+        #                 prev_heading_angle = heading_angle
+        #                 start_idx = i
+        #                 break
+        #         else:
+        #             # 如果没有找到超过1米的距离，使用最后一帧的数据计算航向角
+        #             last_pos_x, last_pos_y = group.iloc[-1]['pos_x'], group.iloc[-1]['pos_y']
+        #             heading_angle = np.degrees(np.arctan2(last_pos_y - start_pos_y, last_pos_x - start_pos_x))
+        #
+        #             # 检查航向角与前一个航向角的差值
+        #             if prev_heading_angle is not None:
+        #                 angle_diff = np.abs(heading_angle - prev_heading_angle)
+        #                 if angle_diff > 180:
+        #                     angle_diff = 360 - angle_diff
+        #
+        #                 if angle_diff <= 20:
+        #                     group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = heading_angle
+        #                 else:
+        #                     # 如果差值大于25度，跳过最后的点
+        #                     group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = prev_heading_angle
+        #             else:
+        #                 group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = heading_angle
+        #
+        #             break
+        #
+        #     # 确保最后一行的航向角等于倒数第二行
+        #     if length > 1:
+        #         group.iloc[-1, group.columns.get_loc('HeadingAngle')] = group.iloc[-2]['HeadingAngle']
+        #
+        #     return group
+
+
         def calculate_heading_angle(group):
             start_idx = 0
             length = len(group)
@@ -172,7 +277,7 @@ class Batchrun:
                     current_pos_x, current_pos_y = group.iloc[i]['pos_x'], group.iloc[i]['pos_y']
                     distance = np.sqrt((current_pos_x - start_pos_x) ** 2 + (current_pos_y - start_pos_y) ** 2)
 
-                    if distance >= 1:
+                    if distance >= 0.5:
                         # 计算航向角
                         heading_angle = np.degrees(np.arctan2(current_pos_y - start_pos_y, current_pos_x - start_pos_x))
 
@@ -180,10 +285,14 @@ class Batchrun:
                         if prev_heading_angle is not None:
                             angle_diff = np.abs(heading_angle - prev_heading_angle)
                             if angle_diff > 180:
+                                if heading_angle < 0:
+                                    heading_angle += 360
+                                else:
+                                    heading_angle -= 360
                                 angle_diff = 360 - angle_diff
 
-                            if angle_diff > 20:
-                                # 如果航向角差值大于25度，跳过当前点
+                            if angle_diff > 30:
+                                # 如果航向角差值大于30度，跳过当前点
                                 continue
 
                         # 将 start_idx 到 i 之间的航向角设置为 heading_angle
@@ -200,15 +309,19 @@ class Batchrun:
                     if prev_heading_angle is not None:
                         angle_diff = np.abs(heading_angle - prev_heading_angle)
                         if angle_diff > 180:
+                            if heading_angle < 0:
+                                heading_angle += angle_diff
+                            else:
+                                heading_angle -= angle_diff
                             angle_diff = 360 - angle_diff
 
-                        if angle_diff <= 20:
+                        if angle_diff <= 30:
                             group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = heading_angle
                         else:
-                            # 如果差值大于25度，跳过最后的点
+                            # 如果差值大于20度，跳过最后的点
                             group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = prev_heading_angle
                     else:
-                        group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = heading_angle
+                        group.iloc[start_idx:length - 1, group.columns.get_loc('HeadingAngle')] = group.iloc[0]['HeadingAngle']
 
                     break
 
@@ -218,17 +331,23 @@ class Batchrun:
 
             return group
 
+        def apply_savgol(row, window_size, poly_order):
+            if len(row) < window_size:
+                return row
+            else:
+                return savgol_filter(row, window_size, poly_order)
+
         # 应用到分组数据
         pos_obs = pos_obs.groupby('ID').apply(calculate_heading_angle).reset_index(drop=True)
-        while (pos_obs['HeadingAngle'] < 0).any():
-            # 将负角度值转换为正值
-            pos_obs['HeadingAngle'] = pos_obs['HeadingAngle'].apply(lambda x: x + 360 if x < 0 else x) 
-
+        # while (pos_obs['HeadingAngle'] < 0).any():
+        #     # 将负角度值转换为正值
+        #     pos_obs['HeadingAngle'] = pos_obs['HeadingAngle'].apply(lambda x: x + 360 if x < 0 else x)
+        pos_obs['HeadingAngle'] = pos_obs.groupby('ID')['HeadingAngle'].transform(lambda x: apply_savgol(x, 45, 2))
         groups = pos_obs.groupby('ID')
         object_points = []
         for key, value in groups:
-            # if len(value) < 5:
-            #     continue
+            if len(value) < 5:
+                continue
             # value = smooth_1(value)
             object_points.append(
                 Scenario.getXoscPosition(value, 'simtime', 'pos_x', 'pos_y', 'HeadingAngle', 'altitude', 'Type', 0, 0, 0,
@@ -324,9 +443,10 @@ class Batchrun:
 
 
 if __name__ == "__main__":
-    # rootPath = "/media/hancheng/Simulation5/pujin/pujin_outdoor/pjioutrobot_2024-08-21-15-12-04"  # 跟车
-    rootPath = sys.argv[1]
-    vehicle_type = sys.argv[2]   # 0 配送 1 巡检
+    rootPath = "/media/hancheng/Simulation5/pujin/pujin_outdoor/10_23"  # 跟车
+    # rootPath = sys.argv[1]
+    # vehicle_type = sys.argv[2]
+    vehicle_type = "0"
     # 生成场景
     a = Batchrun(rootPath, "pos_pji.csv")
     a.batchRun(rootPath, vehicle_type)  # 0为占位参数