|
|
@@ -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
|
|
|
@@ -12,7 +12,8 @@ from datetime import datetime
|
|
|
import argparse
|
|
|
import pandas as pd
|
|
|
import json
|
|
|
-#from nav_msgs.msg import OccupancyGrid
|
|
|
+
|
|
|
+# from nav_msgs.msg import OccupancyGrid
|
|
|
|
|
|
|
|
|
global_height, global_costmap, global_origin, global_resolution = None, None, None, None
|
|
|
@@ -68,7 +69,7 @@ def process_local_rosbag(bagfile, local_topic):
|
|
|
for topic, msg, t in bag.read_messages(topics=local_topic):
|
|
|
local_messages.append((msg, t))
|
|
|
|
|
|
- local_interval = total_time / len(local_messages)
|
|
|
+ local_interval = total_time / (len(local_messages) + 0.01)
|
|
|
|
|
|
for index, (msg, t) in enumerate(local_messages):
|
|
|
local_costmap = np.array(msg.data).reshape(msg.info.height, msg.info.width)
|
|
|
@@ -92,6 +93,7 @@ def process_local_rosbag(bagfile, local_topic):
|
|
|
|
|
|
return df
|
|
|
|
|
|
+
|
|
|
def merge_obstacles(df, global_height):
|
|
|
df.sort_values(by=['frame_time', 'x', 'y'], inplace=True)
|
|
|
|
|
|
@@ -132,210 +134,195 @@ def merge_obstacles(df, global_height):
|
|
|
return result_df
|
|
|
|
|
|
|
|
|
-
|
|
|
-
|
|
|
-def parsezihao(input_dir, output_dir):
|
|
|
- dic_EgoState = ['Time','simTime','simFrame','posX','posY','posZ','speedX','speedY','speedZ','accelX','accelY','accelZ',
|
|
|
- 'dimX','dimY','dimZ','obstacle','traveledDist']
|
|
|
- #dic_DriverCtrl=['Time','simTime','simFrame','tarspeedX','tarspeedY','tarspeedZ','tardimX','tardimY','tardimZ']
|
|
|
- EgoState_file = open(output_dir + "/"+"EgoState_pji.csv", 'w')
|
|
|
- #DriverCtrl_file = open(output_dir + "/"+"DriverCtrl_pji.csv", 'w')
|
|
|
- writer_EgoState = csv.writer(EgoState_file)
|
|
|
- writer_EgoState.writerow(dic_EgoState)
|
|
|
- #writer_DriverCtrl = csv.writer(DriverCtrl_file)
|
|
|
- #writer_DriverCtrl.writerow(dic_DriverCtrl)
|
|
|
-
|
|
|
-
|
|
|
- frame_max=sys.maxsize
|
|
|
- count=1
|
|
|
- with rosbag.Bag(input_dir ,'r') as bag:
|
|
|
- odom_flag=False
|
|
|
- first_message_time = None
|
|
|
- Frame_imu=1
|
|
|
- Frame_cmd_vel=1
|
|
|
- obstacle_state=0
|
|
|
- cur_mileage=''
|
|
|
-
|
|
|
-
|
|
|
- for topic,msg,t in bag.read_messages(topics=['/obstacle_detection','sys_info','odom','imu']): #t代表时间
|
|
|
-
|
|
|
- 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 == "/obstacle_detection":
|
|
|
- obstacle_state=msg.data
|
|
|
- #print(msg.data)
|
|
|
-
|
|
|
- if topic == "/sys_info":
|
|
|
- cur_mileage=msg.cur_mileage
|
|
|
-
|
|
|
-
|
|
|
- if topic == "/odom":
|
|
|
- odom_flag=True
|
|
|
- posX=msg.pose.pose.position.x
|
|
|
- posY=msg.pose.pose.position.y
|
|
|
- posZ=msg.pose.pose.position.z
|
|
|
- speedX=msg.twist.twist.linear.x*3.6
|
|
|
- speedY=msg.twist.twist.linear.y*3.6
|
|
|
- speedZ=msg.twist.twist.linear.z*3.6
|
|
|
- dimX=msg.twist.twist.angular.x
|
|
|
- dimY=msg.twist.twist.angular.y
|
|
|
- dimZ=msg.twist.twist.angular.z
|
|
|
-
|
|
|
- if topic == "/imu":
|
|
|
-
|
|
|
- if odom_flag:
|
|
|
- accelX=msg.linear_acceleration.x
|
|
|
- accelY=msg.linear_acceleration.y
|
|
|
- accelZ=msg.linear_acceleration.z
|
|
|
- timestamp = rospy.Time.to_sec(t)
|
|
|
- date_time_imu = datetime.fromtimestamp(timestamp)
|
|
|
- simtime_imu=(date_time_imu-first_message_time).total_seconds()
|
|
|
-
|
|
|
- message_EgoState =[date_time_imu,simtime_imu,Frame_imu,posX,posY,posZ,speedX,speedY,speedZ,accelX,accelY,
|
|
|
- accelZ,dimX,dimY,dimZ,obstacle_state,cur_mileage]
|
|
|
-
|
|
|
- writer_EgoState.writerow(message_EgoState)
|
|
|
- Frame_imu+=1
|
|
|
- else:
|
|
|
- print('6666')
|
|
|
- '''
|
|
|
- if topic =='/cmd_vel':
|
|
|
- timestamp = rospy.Time.to_sec(t)
|
|
|
- date_time_cmd_vel = datetime.fromtimestamp(timestamp)
|
|
|
- simtime_cmd_vel=(date_time_cmd_vel-first_message_time).total_seconds()
|
|
|
- message_DriverCtrl =[date_time_cmd_vel,simtime_cmd_vel,Frame_cmd_vel,msg.linear.x*3.6,msg.linear.y*3.6,msg.linear.z*3.6,
|
|
|
- msg.angular.x,msg.angular.y,msg.angular.z]
|
|
|
-
|
|
|
-
|
|
|
- writer_DriverCtrl.writerow(message_DriverCtrl)
|
|
|
- Frame_cmd_vel+=1
|
|
|
- '''
|
|
|
-
|
|
|
- EgoState_file.close()
|
|
|
- #DriverCtrl_file.close()
|
|
|
-
|
|
|
-
|
|
|
-def parsehancheng(input_dir, output_dir):
|
|
|
+def parsehancheng(input_dir, output_dir):
|
|
|
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
|
|
|
|
|
|
- json_path=os.path.join(output_dir,'output.json')
|
|
|
- dic_object_detection = ['Time','FrameID','HeadingAngle','X', 'Y' ,'Z']
|
|
|
- object_detection_file = open(output_dir + "/"+"pos_pji.csv", 'w')
|
|
|
+ json_path = os.path.join(output_dir, 'output.json')
|
|
|
+ dic_object_detection = ['Time', 'FrameID', 'HeadingAngle', 'X', 'Y', 'Z']
|
|
|
+ object_detection_file = open(output_dir + "/" + "pos_pji.csv", 'w')
|
|
|
writer_object_detection = csv.writer(object_detection_file)
|
|
|
writer_object_detection.writerow(dic_object_detection)
|
|
|
|
|
|
+ dic_EgoState = ['Time', 'simTime', 'simFrame', 'posX', 'posY', 'posZ', 'speedX', 'speedY', 'speedZ', 'accelX',
|
|
|
+ 'accelY', 'accelZ',
|
|
|
+ 'dimX', 'dimY', 'dimZ', 'obstacle', 'traveledDist']
|
|
|
+ # dic_DriverCtrl=['Time','simTime','simFrame','tarspeedX','tarspeedY','tarspeedZ','tardimX','tardimY','tardimZ']
|
|
|
+ EgoState_file = open(output_dir + "/" + "EgoState_pji.csv", 'w')
|
|
|
+ # DriverCtrl_file = open(output_dir + "/"+"DriverCtrl_pji.csv", 'w')
|
|
|
+ writer_EgoState = csv.writer(EgoState_file)
|
|
|
+ writer_EgoState.writerow(dic_EgoState)
|
|
|
+ # writer_DriverCtrl = csv.writer(DriverCtrl_file)
|
|
|
+ # writer_DriverCtrl.writerow(dic_DriverCtrl)
|
|
|
+
|
|
|
+ frame_max = sys.maxsize
|
|
|
+ count = 1
|
|
|
|
|
|
- frame_max=sys.maxsize
|
|
|
- with rosbag.Bag(input_dir ,'r') as bag:
|
|
|
- poseX=poseY=0
|
|
|
- #flag=False
|
|
|
+ frame_max = sys.maxsize
|
|
|
+ with rosbag.Bag(input_dir, 'r') as bag:
|
|
|
+ poseX = poseY = 0
|
|
|
+ # flag=False
|
|
|
framenum = 1
|
|
|
- #hasLoc = False
|
|
|
- #用来判断机器人点云/图像/规划/定位是否丢帧↓↓↓
|
|
|
- num_cam1=0
|
|
|
- rate_cam1=10
|
|
|
-
|
|
|
- num_cam2=0
|
|
|
- rate_cam2=10
|
|
|
-
|
|
|
- cam1_exist_flag=False
|
|
|
- cam2_exist_flag=False
|
|
|
-
|
|
|
- #num_pcd=0
|
|
|
- #rate_pcd=10
|
|
|
-
|
|
|
+ # hasLoc = False
|
|
|
+ # 用来判断机器人点云/图像/规划/定位是否丢帧↓↓↓
|
|
|
+ num_cam1 = 0
|
|
|
+ rate_cam1 = 10
|
|
|
+
|
|
|
+ num_cam2 = 0
|
|
|
+ rate_cam2 = 10
|
|
|
+
|
|
|
+ cam1_exist_flag = False
|
|
|
+ cam2_exist_flag = False
|
|
|
+
|
|
|
+ # num_pcd=0
|
|
|
+ # rate_pcd=10
|
|
|
+
|
|
|
bag_start_time = bag.get_start_time()
|
|
|
bag_end_time = bag.get_end_time()
|
|
|
- duration=bag_end_time-bag_start_time
|
|
|
-
|
|
|
- #Theoretical_pcd_num=int(duration*rate_pcd)
|
|
|
- Theoretical_cam1_num=int(duration*rate_cam1)
|
|
|
- Theoretical_cam2_num=int(duration*rate_cam2)
|
|
|
-
|
|
|
- amcl_pose_lost_flag=True
|
|
|
-
|
|
|
-
|
|
|
- #用来判断机器人点云/图像/规划/定位是否丢帧↑↑↑
|
|
|
- for topic,msg,t in bag.read_messages(topics=['/amcl_pose','/scan_map_icp_amcl_node/scan_point_transformed','/ob_camera_01/color/image_raw','/ob_camera_02/color/image_raw']): #t代表时间
|
|
|
+ duration = bag_end_time - bag_start_time
|
|
|
+
|
|
|
+ # Theoretical_pcd_num=int(duration*rate_pcd)
|
|
|
+ Theoretical_cam1_num = int(duration * rate_cam1)
|
|
|
+ Theoretical_cam2_num = int(duration * rate_cam2)
|
|
|
+
|
|
|
+ amcl_pose_lost_flag = True
|
|
|
+
|
|
|
+ odom_flag = False
|
|
|
+ first_message_time = None
|
|
|
+ Frame_imu = 1
|
|
|
+ Frame_cmd_vel = 1
|
|
|
+ obstacle_state = 0
|
|
|
+ cur_mileage = 0
|
|
|
+ imu_odom_flag = False
|
|
|
+
|
|
|
+ # 用来判断机器人点云/图像/规划/定位是否丢帧↑↑↑
|
|
|
+ for topic, msg, t in bag.read_messages(
|
|
|
+ topics=['/amcl_pose', '/scan_map_icp_amcl_node/scan_point_transformed', '/ob_camera_01/color/image_raw',
|
|
|
+ '/ob_camera_02/color/image_raw', '/obstacle_detection', '/sys_info', '/odom', '/imu']): # t代表时间
|
|
|
'''
|
|
|
if topic == "/scan_map_icp_amcl_node/scan_point_transformed":
|
|
|
num_pcd+=1
|
|
|
- '''
|
|
|
+ '''
|
|
|
if topic == "/ob_camera_01/color/image_raw":
|
|
|
- cam1_exist_flag=True
|
|
|
- num_cam1+=1
|
|
|
-
|
|
|
+ cam1_exist_flag = True
|
|
|
+ num_cam1 += 1
|
|
|
+
|
|
|
if topic == "/ob_camera_01/color/image_raw":
|
|
|
- cam2_exist_flag=True
|
|
|
- num_cam2+=1
|
|
|
-
|
|
|
- if topic == "/amcl_pose":#100hz /odom
|
|
|
-
|
|
|
- poseX=msg.pose.pose.position.x
|
|
|
- poseY=msg.pose.pose.position.y
|
|
|
- poseZ=msg.pose.pose.position.z
|
|
|
- if poseX!=0 and poseY!=0:
|
|
|
- amcl_pose_lost_flag=False
|
|
|
- orientationX=msg.pose.pose.orientation.x
|
|
|
- orientationY=msg.pose.pose.orientation.y
|
|
|
- orientationZ=msg.pose.pose.orientation.z
|
|
|
- orientationW=msg.pose.pose.orientation.w
|
|
|
- egoyaw=quaternion_to_euler(orientationX,orientationY,orientationZ,orientationW)
|
|
|
- message_location =[str(t)[:-6],framenum,egoyaw,poseX,poseY,poseZ]
|
|
|
-
|
|
|
+ cam2_exist_flag = True
|
|
|
+ num_cam2 += 1
|
|
|
+
|
|
|
+ if topic == "/amcl_pose": # 100hz /odom
|
|
|
+
|
|
|
+ poseX = msg.pose.pose.position.x
|
|
|
+ poseY = msg.pose.pose.position.y
|
|
|
+ poseZ = msg.pose.pose.position.z
|
|
|
+ if poseX != 0 and poseY != 0:
|
|
|
+ amcl_pose_lost_flag = False
|
|
|
+ orientationX = msg.pose.pose.orientation.x
|
|
|
+ orientationY = msg.pose.pose.orientation.y
|
|
|
+ orientationZ = msg.pose.pose.orientation.z
|
|
|
+ orientationW = msg.pose.pose.orientation.w
|
|
|
+ egoyaw = quaternion_to_euler(orientationX, orientationY, orientationZ, orientationW)
|
|
|
+ message_location = [str(t)[:-6], framenum, egoyaw, poseX, poseY, poseZ]
|
|
|
+
|
|
|
writer_object_detection.writerow(message_location)
|
|
|
- framenum+=1
|
|
|
+ framenum += 1
|
|
|
+
|
|
|
+ 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 == "/obstacle_detection":
|
|
|
+ obstacle_state = msg.data
|
|
|
+ # print(msg.data)
|
|
|
+
|
|
|
+ if topic == "/sys_info":
|
|
|
+ cur_mileage = msg.cur_mileage
|
|
|
+
|
|
|
+ if topic == "/odom":
|
|
|
+ odom_flag = True
|
|
|
+ posX = msg.pose.pose.position.x
|
|
|
+ posY = msg.pose.pose.position.y
|
|
|
+ posZ = msg.pose.pose.position.z
|
|
|
+ speedX = msg.twist.twist.linear.x * 3.6
|
|
|
+ speedY = msg.twist.twist.linear.y * 3.6
|
|
|
+ speedZ = msg.twist.twist.linear.z * 3.6
|
|
|
+ dimX = msg.twist.twist.angular.x
|
|
|
+ dimY = msg.twist.twist.angular.y
|
|
|
+ dimZ = msg.twist.twist.angular.z
|
|
|
+
|
|
|
+ if topic == "/imu":
|
|
|
+
|
|
|
+ if odom_flag:
|
|
|
+ # print(posX)
|
|
|
+ accelX = msg.linear_acceleration.x
|
|
|
+ accelY = msg.linear_acceleration.y
|
|
|
+ accelZ = msg.linear_acceleration.z
|
|
|
+ timestamp = rospy.Time.to_sec(t)
|
|
|
+ date_time_imu = datetime.fromtimestamp(timestamp)
|
|
|
+ simtime_imu = (date_time_imu - first_message_time).total_seconds()
|
|
|
+
|
|
|
+ message_EgoState = [date_time_imu, simtime_imu, Frame_imu, posX, posY, posZ, speedX, speedY, speedZ,
|
|
|
+ accelX, accelY,
|
|
|
+ accelZ, dimX, dimY, dimZ, obstacle_state, cur_mileage]
|
|
|
+
|
|
|
+ writer_EgoState.writerow(message_EgoState)
|
|
|
+ imu_odom_flag = True
|
|
|
+ Frame_imu += 1
|
|
|
+
|
|
|
object_detection_file.close()
|
|
|
-
|
|
|
+ EgoState_file.close()
|
|
|
+
|
|
|
with open(json_path, "w") as file:
|
|
|
data = []
|
|
|
'''
|
|
|
if (Theoretical_pcd_num - num_pcd) / Theoretical_pcd_num > 0.5:
|
|
|
data.append('点云丢帧')
|
|
|
'''
|
|
|
- if cam1_exist_flag==False and cam2_exist_flag==False:
|
|
|
+ if cam1_exist_flag == False and cam2_exist_flag == False:
|
|
|
data.append('图像缺失')
|
|
|
- if cam1_exist_flag==True and cam2_exist_flag==True:
|
|
|
- if ((Theoretical_cam1_num - num_cam1) / Theoretical_cam1_num > 0.5) or ((Theoretical_cam2_num - num_cam2) / Theoretical_cam2_num > 0.5):
|
|
|
+ if cam1_exist_flag == True and cam2_exist_flag == True:
|
|
|
+ if ((Theoretical_cam1_num - num_cam1) / Theoretical_cam1_num > 0.5) or (
|
|
|
+ (Theoretical_cam2_num - num_cam2) / Theoretical_cam2_num > 0.5):
|
|
|
data.append('图像丢帧')
|
|
|
- if amcl_pose_lost_flag :
|
|
|
+ if amcl_pose_lost_flag:
|
|
|
+ data.append('位姿信息缺失')
|
|
|
+ if not imu_odom_flag:
|
|
|
data.append('自车数据缺失')
|
|
|
-
|
|
|
+
|
|
|
if data == []:
|
|
|
data = ['正常']
|
|
|
-
|
|
|
+
|
|
|
# 将数据转换为 JSON 格式并写入文件
|
|
|
json.dump(data, file, ensure_ascii=False)
|
|
|
|
|
|
+
|
|
|
def parse(costmap_bag, data_bag, output_dir):
|
|
|
global_bagfile = costmap_bag # 全局地图的rosbag包
|
|
|
# global_bagfile = '/media/dell/56FAD34BFAD32651/data/pujin_datareturn/20240428/costmap.bag'
|
|
|
input_dir = data_bag # 触发采集的rosbag包
|
|
|
- #input_dir='/media/dell/56FAD34BFAD32651/data/pujin_datareturn/20240428/2024-04-25-15-58-27_obstacledetection_30.bag'
|
|
|
- bagname=input_dir.split('/')[-1].split('.')[0]
|
|
|
- #output_dir='/media/dell/56FAD34BFAD32651/data/pujin_datareturn/20240428'
|
|
|
+ # input_dir='/media/dell/56FAD34BFAD32651/data/pujin_datareturn/20240428/2024-04-25-15-58-27_obstacledetection_30.bag'
|
|
|
+ bagname = input_dir.split('/')[-1].split('.')[0]
|
|
|
+ # output_dir='/media/dell/56FAD34BFAD32651/data/pujin_datareturn/20240428'
|
|
|
global_topic = '/move_base/global_costmap/costmap'
|
|
|
local_topic = '/move_base/local_costmap/costmap'
|
|
|
-
|
|
|
- output_dir=os.path.join(output_dir, bagname)
|
|
|
+
|
|
|
+ output_dir = os.path.join(output_dir, bagname)
|
|
|
if not os.path.exists(output_dir):
|
|
|
os.makedirs(output_dir)
|
|
|
'''
|
|
|
@@ -347,18 +334,23 @@ def parse(costmap_bag, data_bag, output_dir):
|
|
|
print('successfully analysis '+input_dir)
|
|
|
'''
|
|
|
|
|
|
- try:
|
|
|
- process_global_rosbag(global_bagfile, global_topic)
|
|
|
- df = process_local_rosbag(input_dir, local_topic)
|
|
|
- result_df = merge_obstacles(df, global_height)
|
|
|
- result_df.to_csv(os.path.join(output_dir,'merged_obstacles.csv'), index=False)
|
|
|
- parsehancheng(input_dir, output_dir)
|
|
|
- parsezihao(input_dir, output_dir)
|
|
|
- print('successfully analysis '+input_dir)
|
|
|
+ # try:
|
|
|
+ process_global_rosbag(global_bagfile, global_topic)
|
|
|
+ df = process_local_rosbag(input_dir, local_topic)
|
|
|
+ result_df = merge_obstacles(df, global_height)
|
|
|
+ result_df.to_csv(os.path.join(output_dir, 'merged_obstacles.csv'), index=False)
|
|
|
+ parsehancheng(input_dir, output_dir)
|
|
|
+
|
|
|
+ print('successfully analysis ' + input_dir)
|
|
|
+ '''
|
|
|
except Exception as e:
|
|
|
print(e)
|
|
|
json_path=os.path.join(output_dir,'output.json')
|
|
|
with open(json_path, "w") as file:
|
|
|
- data = ['解析程序错误']
|
|
|
- # 将数据转换为 JSON 格式并写入文件
|
|
|
- json.dump(data, file, ensure_ascii=False)
|
|
|
+ data = ['解析程序错误']
|
|
|
+
|
|
|
+
|
|
|
+ # 将数据转换为 JSON 格式并写入文件
|
|
|
+ json.dump(data, file, ensure_ascii=False)
|
|
|
+
|
|
|
+ '''
|
