Ego Ghost Mode 사용 방법
본 페이지는 User가 원하는 위치/자세로 Ego Vehicle을 생성 할 수 있는 모드인 Ego Ghost Mode에 대해 소개한다.
ROS Protocol Messages
https://github.com/morai-developergroup/morai_msgs/blob/master/msg/GhostMessage.msg
Ghost Ctrl Cmd
Ghost Mode Ego 차량 제어 명령
Message Type : morai_msgs/GhostMessage
Default Topic : /ghost_ctrl_cmd
타입 설명 : Ghost Mode 이용 Ego 차량을 제어하기 위한 메세지
Ghost Mode : 차량을 원하는 위치에 생성 할 수 있는 모드이다.
No | Name | Type | Unit | Remarks |
|---|---|---|---|---|
1 | position | m | ego 차량의 위치 지정 (X, Y, Z) | |
2 | rotation | deg | ego 차량의 회전 지정 (roll, pitch, yaw) | |
3 | velocity | float64 | km/h | ego 차량의 속도 |
4 | steering_angle | float64 | deg | ego 차량 앞바퀴 조향 각도 |
Ego Ghost Mode 기능 사용법
Edit > Network Settings의 Cmd Control에서 MoraiGhostCmdController를 선택하여 Ego Ghost Mode 설정 가능함.
사용 환경에 맞게 IP를 적어주고 Connect함.
Network Setting
ROS에서 /ghost_ctrl_cmd _Topic을 Publish 함.
Publish 후 터미널
Keyboard Q를 누르면 Publish한 정보에 따라 Ego Ghost Vehicle이 생성되는 것 확인 가능함.
Keyboard Q 누른 후 Ego Vehicle의 모습
UDP Protocol Messages
Ghost Ctrl Cmd
타입 설명 : Ghost Mode 이용 Ego 차량을 제어하기 위한 메세지
통신 프로토콜
전체 패킷 크기: 63 Bytes
데이터 크기 32 Bytes
x_position (4byte / float)
ego 차량의 x 위치 지정 (m)
y_position (4byte / float)
ego 차량의 y 위치 지정 (m)
z_position (4byte / float)
ego 차량의 z 위치 지정 (m)
roll_rotation (4byte / float)
ego 차량의 roll 회전 지정 (deg)
pitch_rotation (4byte / float)
ego 차량의 pitch 회전 지정 (deg)
yaw_rotation (4byte / float)
ego 차량의 yaw 회전 지정 (deg)
velocity (4byte / float)
ego 차량의 속도 (Km/h)
steering_angle (4byte / float)
ego 차량 앞바퀴 조향 각도 (deg)
Ego Ghost Mode 기능 사용법
Ego 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"]
ego_ghost_data_port = params["ego_ghost_host_port"]
status_port = params["vehicle_status_dst_port"]
class ego_ghost_host_port :
def __init__(self):
self.ego_ghost_host_port=udp_sender(user_ip,ego_ghost_data_port,'ego_ghost_cmd')
self.status = udp_parser(host_ip, status_port, 'status')
self._is_status = False
while not self._is_status:
if not self.status.get_data():
print('No Status Data Cannot run main_loop')
time.sleep(1)
else:
self._is_status = True
self.main_loop()
def main_loop(self):
self.timer=threading.Timer(0.1,self.main_loop)
self.timer.start()
self.test = 0
status_data = self.status.get_data()
print(status_data)
ego_ghost_data=[]
# (unique_id), pose X, pose Y, pose Z, roll, Pitch, Yaw, Speed_ego, Steering_ego
while True:
ego_1=20
ego_2=1100
ego_3=0.7
ego_4=0
ego_5=0
ego_6=0
ego_7=0
ego_8=0
ego_9=0
ego_10=0
ego_ghost_data.append(ego_1)
ego_ghost_data.append(ego_2)
ego_ghost_data.append(ego_3)
ego_ghost_data.append(ego_4)
ego_ghost_data.append(ego_5)
ego_ghost_data.append(ego_6)
ego_ghost_data.append(ego_7)
ego_ghost_data.append(ego_8)
ego_ghost_data.append(ego_9)
ego_ghost_data.append(ego_10)
self.test += 10
self.ego_ghost_host_port.send_data(ego_ghost_data)
print(self.test)
###################
if __name__ == "__main__":
ego_ghost_host_port=ego_ghost_host_port()
while True :
pass
CODE
MORAI 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()
################## ego_ghost_cmd ##################
elif self.data_type == 'ego_ghost_cmd':
header='#EgoGhostCmd$'.encode()
data_length=struct.pack('i',4)
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)
################## ego_ghost_cmd ##################
elif self.data_type == 'ego_ghost_cmd':
lower=None
for ego in range(32) :
if ego <len(data):
print(ego,data[ego][0],data[ego][1])
packed_PoseX=struct.pack('f',data[ego][0])
packed_Posey=struct.pack('f',data[ego][1])
packed_Posez=struct.pack('f',data[ego][2])
packed_roll=struct.pack('f',data[ego][3])
packed_pitch=struct.pack('f',data[ego][4])
packed_yaw=struct.pack('f',data[ego][5])
packed_speed_ego=struct.pack('f',data[ego][6])
packed_steering_ego=struct.pack('f',data[ego][7])
packed_steering_ego1=struct.pack('f',data[ego][8])
packed_steering_ego2=struct.pack('f',data[ego][9])
lower=packed_PoseX+packed_Posey+packed_Posez+packed_roll+packed_pitch+packed_yaw+packed_speed_ego+packed_steering_ego
status_data=struct.pack('ffffffff',data[ego][0],data[ego][1],data[ego][2],data[ego][3],data[ego][4],data[ego][5],data[ego][6],data[ego][7],data[ego][8],data[ego][9])
pack_data=status_data
else:
status_data=struct.pack('ffffffff',0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0)
pack_data=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))
CODE