commit

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