NPC Ghost Mode 사용 방법
본 페이지는 User가 원하는 위치/자세로 NPC Vehicle을 생성 할 수 있는 모드인 NPC Ghost Mode에 대해 소개한다.
ROS Protocol Messages
NPC Ghost Controller
https://github.com/morai-developergroup/morai_msgs/blob/master/msg/NpcGhostCmd.msg
NPC Ghost Controller
Message Type : morai_msgs/NpcGhostCmd
Default Topic : /NpcGhost_Topic
타입설명 : Npc Ghost mode에서 Npc Vehicle을 생성(Control) 하는 메세지
No | Name | Type | Unit | Remarks |
|---|---|---|---|---|
1 | header | Header | - | |
2 | npc_list | - | Npc Ghost Vehicle 정보 |
NPC Ghost Mode 기능 사용법
NPC Ghost Mode 설정은 시뮬레이터 상단 PlayMode 메뉴의 하위 메뉴에서 설정이 가능하다.
NPC Ghost Mode Settings 창에서 사용 환경에 맞게 ROS Bridge의 IP를 적어주고 Connect를 눌러준다.
Status UI를 통해 연결 유무를 확인할 수 있다.
ROS에서 /NpcGhost_Topic을 Publish 해주면 해당 정보에 따라 NPC Ghost Vehicle이 생성되는 것을 확인 할 수 있다.
UDP Protocol Messages
NPC Ghost Controller
타입 설명
Ghost Mode 이용 NPC 차량들을 표현하기 위한 메세지
최대 20대까지 표현이 가능
만약 20대 미만의 NPC를 생성하고 싶은 경우 불필요한 차량 정보에 대한 unique_id를 0으로 지정하고, 임의의 데이터들로 채우면 됨
통신 프로토콜
전체 패킷 크기: 1031 Bytes
데이터 크기 1000 Bytes (50 Bytes * 20)
unique_id (1byte / int8)
Npc Ghost Vehicle의 unique_id 값 (e.g. 2,3,4..)
car_name (25byte / string)
Npc Ghost Vehicle의 모델 이름
입력할 데이터 크기는 25byte로 25byte 보다 짧을 경우 뒤에 Space로 공백을 채워야 하며, 길면 줄여 25byte에 맞춰야함.e.g.
"2016_Kia_Niro(HEV) "
x_position (4byte / float)
Npc Ghost Vehicle의 x 위치 지정 (m)
y_position (4byte / float)
Npc Ghost Vehicle의 y 위치 지정 (m)
z_position (4byte / float)
Npc Ghost Vehicle의 z 위치 지정 (m)
roll_rotation (4byte / float)
Npc Ghost Vehicle의 roll 회전 지정 (deg)
pitch_rotation (4byte / float)
Npc Ghost Vehicle의 pitch 회전 지정 (deg)
yaw_rotation (4byte / float)
Npc Ghost Vehicle의 yaw 회전 지정 (deg)
NPC Ghost Mode 기능 사용법
NPC Ghost Cmd.py
from lib.morai_udp_parser import udp_parser,udp_sender
from lib.utils import pathReader,findLocalPath,purePursuit,Point,cruiseControl,vaildObject,velocityPlanning,pidController
import time
import threading
from math import cos,sin,sqrt,pow,atan2,pi
import os,json
path = os.path.dirname( os.path.abspath( __file__ ) )
with open(os.path.join(path,("params.json")),'r') as fp :
params = json.load(fp)
params=params["params"]
user_ip = params["user_ip"]
host_ip = params["host_ip"]
npc_ghost_data_port = params["npc_ghost_host_port"]
class npc_ghost_host_port :
def __init__(self):
self.npc_ghost_host_port=udp_sender(user_ip,npc_ghost_data_port,'npc_ghost_cmd')
self.emptyNpcData = []
#self.main_loop()
self.timer=threading.Timer(0.1,self.main_loop)
self.npc_ghost_data=[]
self.emptyNpcData.append(0)
self.emptyNpcData.append(bytes([0]*25))
for i in range(6):
self.emptyNpcData.append(0)
tmpNpcData = []
npc_0=2
modelName = "2017_Kia_Sorento"
tmpNameLen = len(modelName)
npc_1 = bytearray(modelName, "utf-8") + bytearray([0]* (25 - tmpNameLen))
#npc_1=bytearray("2017_Kia_Niro(HEV)","utf-8")+(bytearray([0]*6))
npc_2=20.0
npc_3=1100.0
npc_4=0.7
npc_5=0
npc_6=0
npc_7=0
tmpNpcData.append(npc_0)
tmpNpcData.append(npc_1)
tmpNpcData.append(npc_2)
tmpNpcData.append(npc_3)
tmpNpcData.append(npc_4)
tmpNpcData.append(npc_5)
tmpNpcData.append(npc_6)
tmpNpcData.append(npc_7)
self.npc_ghost_data.append(tmpNpcData)
for j in range(19):
self.npc_ghost_data.append(self.emptyNpcData)
self.timer.start()
def main_loop(self):
# unique_id, pose X, pose Y, pose Z, roll, Pitch, Yaw
#while True:
self.npc_ghost_host_port.send_data(self.npc_ghost_data)
###################
if __name__ == "__main__":
npc_ghost_host_port=npc_ghost_host_port()
while True :
time.sleep(1)
passMORAI UDP Parser.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import socket
import threading
import time
import struct
class udp_parser :
def __init__(self,ip,port,data_type):
print("ip",ip)
print("port",port)
self.data_type=data_type
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
recv_address = (ip,port)
self.sock.bind(recv_address)
self.data_size=65535
self.parsed_data=[]
thread = threading.Thread(target=self.recv_udp_data)
thread.daemon = True
thread.start()
def recv_udp_data(self):
while True :
raw_data, sender = self.sock.recvfrom(self.data_size)
self.data_parsing(raw_data)
def data_parsing(self,raw_data) :
if self.data_type == 'status' :
header=raw_data[0:11].decode()
data_length=struct.unpack('i',raw_data[11:15])
if header == '#MoraiInfo$' : # and data_length[0] ==32:
vgen_ctrl_cmd = struct.unpack('b',raw_data[15:16])
vgen_gear = struct.unpack('b',raw_data[16:17])
unpacked_data_1 = struct.unpack('fi',raw_data[17:25])
unpacked_data_2 = struct.unpack('ffffffff',raw_data[27:59])
unpacked_data = vgen_ctrl_cmd + vgen_gear + unpacked_data_1 + unpacked_data_2
# unpacked_data=struct.unpack('ffffffff',raw_data[27:59])
self.parsed_data=list(unpacked_data)
def get_data(self) :
return self.parsed_data
def __del__(self):
self.sock.close()
print('del')
class udp_sender :
def __init__(self,ip,port,data_type):
print("ip : ", ip)
print("port : ", port)
print("data_type : ", data_type)
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.ip=ip
self.port=port
self.data_type=data_type
if self.data_type=='ctrl_cmd':
header='#MoraiCtrlCmd$'.encode()
data_length=struct.pack('i',12)
# aux_data=struct.pack('iii',0,0,0)
self.upper=header+data_length # +aux_data
self.tail='\r\n'.encode()
################## npc_ghost_cmd ##################
elif self.data_type == 'npc_ghost_cmd':
header='#NpcGhostCmd$'.encode()
data_length=struct.pack('<i',1000)
aux_data=struct.pack('iii',0,0,0)
self.upper=header+data_length+aux_data
self.tail='\r\n'.encode()
def send_data(self,data):
print('1')
if self.data_type=='ctrl_cmd':
packed_mode=struct.pack('b',data[0])
packed_gear=struct.pack('b',data[1])
aux_data1=struct.pack('h',0)
aux_data2=struct.pack('ii',0,0)
packed_accel=struct.pack('f',data[2])
packed_brake=struct.pack('f',data[3])
packed_steering_angle=struct.pack('f',data[4])
lower=packed_mode+packed_gear+aux_data1+aux_data2+packed_accel+packed_brake+packed_steering_angle
send_data=self.upper+lower+self.tail
# print(len(send_data),send_data)
################## npc_ghost_cmd ##################
elif self.data_type == 'npc_ghost_cmd':
print('1')
lower=None
for npc in range(20) :
if npc <len(data):
npc_unique_id=struct.pack('<h',data[npc][0])
#car_name=struct.pack('<25s',data[npc][1])
status_data=struct.pack('<6f',data[npc][2],data[npc][3],data[npc][4],data[npc][5],data[npc][6],data[npc][7])
#pack_data=npc_unique_id+car_name+status_data
pack_data=npc_unique_id+status_data
else:
npc_index=struct.pack('h',0)
#car_name=struct.pack('<25s','')
status_data=struct.pack('ffffff',0.0,0.0,0.0,0.0,0.0,0.0)
pack_data=npc_index+status_data
if lower==None :
lower=pack_data
else :
lower+=pack_data
send_data=self.upper+lower+self.tail
print("send_data : ", send_data)
self.sock.sendto(send_data,(self.ip,self.port))