ngsim读取与ros可视化

NGSIM下载

数据 下载地址 ,如图,选择export,然后下载csv格式,大概1.5G左右

pandas读取与使用

import pandas as pd
#csv_fname = '../Next_Generation_Simulation__NGSIM__Vehicle_Trajectories_and_Supporting_Data.csv'
csv_fname = '../data.txt'
data = pd.read_csv(csv_fname)
#根据frame_id进行升序排序
data.sort_values("Frame_ID",inplace=True)
#筛选id=354的数据
obd_data = data[data["Vehicle_ID"]==354]
#获取行数,列数
rows = obd_data.shape[0]
cols = obd_data.shape[1]
#根据Frame_ID进行统计,各个Frame_ID的item数目
print(data['Frame_ID'].value_counts())
#遍历每一行数据
for row in data.iterrows():
        # feature extraction
        row = row[1]
        frame_id, obs_id, timestamp = row["Frame_ID"], row['Vehicle_ID'], row["Global_Time"]*1e-6

ROS显示

1)自定义消息
根据教程自定义rosmsg,然后编译,使用rosmsg list | grep Num查看是否成功加入rosmsg中.
python中调用方式如下:

#beginner_tutorials是该msg所在的package名称
from  beginner_tutorials.msg import Num 
car_speed = Num() 

2)发送消息
示例代码中提供了MarkerArray的可视化方式, 另外网上还有rviz相关的工具

import rospy
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
count = 0
MARKERS_MAX = 100
topic = 'visualization_marker_array'
publisher = rospy.Publisher(topic, MarkerArray)
markerArray = MarkerArray()
def marker_show(x, y):
    global count
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.color.a = 1.0
    marker.color.r = 1.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = x  # math.cos(count / 50.0)
    marker.pose.position.y = y  # math.cos(count / 40.0)
    marker.pose.position.z = 0  # math.cos(count / 30.0)
# We add the new marker to the MarkerArray, removing the oldest marker from it when necessary
    if(count > MARKERS_MAX):
        markerArray.markers.pop(0)
    markerArray.markers.append(marker)
    # Renumber the marker IDs
    id = 0
    for m in markerArray.markers:
        m.id = id
        id += 1
    # Publish the MarkerArray
    publisher.publish(markerArray)
    count += 1