[예제]MSC API 및 UDP로 시뮬레이터 제어하기

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import time,os,json,threading
from lib.utils import *

from lib.define import *


from lib.controller import *


class launcher_start(controller):
        
    def __init__(self):                
        self.controller = controller()

    def launcher_start(self):
        
        while True:           
            
            if self.controller.update():             
                self.controller.is_waitting() #status_wait 
                self.controller.is_downloading() #check_is_download_status                                      

                if self.controller.is_befor_login():
                                        
                    self.controller.commander(Command.LOGIN)#Login명령
                    
                
                if self.controller.is_after_login() or self.controller.is_after_sim_quit_to_launcher():  # is_after_sim_quit_to_launcher : Simulator에서 quit 명령 후 Launcher 복귀 상태 확인

                    self.controller.commander(Command.SELECT_VER)#version select명령

                if self.controller.is_not_find_version(): #version_error         
                    break
                    
                if self.controller.is_can_execute_sim(): 
                                                                    
                    self.controller.commander(Command.EXECUTE_SIM) #Simulator 실행 명령                        

                    self.controller.watting_execute()
                                            
                if self.controller.is_sim_not_install():                                
                    self.controller.commander(Command.INSTALL_SIM) #Simulator 설치 명령                     


                if self.controller.is_sim_lobby():                    

                    self.controller.commander(Command.MAP_VEHICLE_SELECT)#시뮬레이션/옵션 변경 실행 명령

                    self.controller.watting_loading()

                if self.controller.is_sim_playing():          

                    self.controller.commander(Command.NET_SETTING) #Network setting
                    
                    self.controller.commander(Command.SEN_SETTING) #Sensor setting

                    self.controller.commander(Command.SCEN_SETTING) #Scenraio setting
                    planner(self.controller)
                    break

            else :
                print("[NO Simulator Control Data]")
                time.sleep(1)            



class planner:

    def __init__(self,controller):
        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"]


        status_port =params["vehicle_status_dst_port"]
        object_port =params["object_info_dst_port"]
        get_traffic_port=params["get_traffic_dst_port"]

        set_traffic_port=params["set_traffic_host_port"]
        ctrl_cmd_port = params["ctrl_cmd_host_port"]

        planner_path_file_name = params["planner_path_file_name"]

        traffic_greenlight_setting= params["traffic_greenlight_setting"]
        
        
        self.status=udp_parser(user_ip, status_port,'status')
        self.obj=udp_parser(user_ip, object_port,'obj')
        self.traffic=udp_parser(user_ip, get_traffic_port,'get_traffic')


        self.ctrl_cmd=udp_sender(host_ip,ctrl_cmd_port,'ctrl_cmd')
        self.set_traffic=udp_sender(host_ip,set_traffic_port,'set_traffic')
        

        self.txt_reader=pathReader()
        self.global_path=self.txt_reader.read(planner_path_file_name)


        vel_planner=velocityPlanning(60,2) ## 경로기반 속도 계획
        self.vel_profile=vel_planner.curveBasedVelocity(self.global_path,50)        

        self.pure_pursuit=purePursuit()
        self.cc=cruiseControl(0.5,1) ## cruiseControl import (object_vel_gain, object_dis_gain)
        
        self.vo=vaildObject()## 장애물 유무 확인

        self.pid=pidController() ## pidController import        
        self.current_waypoint=0
  

        self._is_status=False
        while not self._is_status :
            # if not self.status.get_data() :
            controller.commander(Command.SCEN_SETTING)
            print('No Status Data Cannot run main_loop')
            time.sleep(0.5)
        # else :
            self._is_status=True                

        self.main_loop()

    
    def main_loop(self):        
        
        self.timer=threading.Timer(0.1,self.main_loop)
        self.timer.start()
        
        status_data=self.status.get_data()
        obj_data=self.obj.get_data()
        traffic_data = self.traffic.get_data()
        position_x=status_data[4]
        position_y=status_data[5]
        position_z=status_data[6]
        heading=status_data[9]# degree
        velocity=status_data[10]

        #set trafficlight (green)
        if not len(traffic_data) == 0 and traffic_greenlight_setting == "True": #set trafficlight (green)                        
            self.set_traffic.send_data([False,traffic_data[1],16])
            traffic_data[3]=16


        #fine_local_path, waypoint
        local_path,current_waypoint =findLocalPath(self.global_path,position_x,position_y)
        
        
        ## 장애물의 숫자와 Type 위치 속도 (object_num, object type, object pose_x, object pose_y, object velocity)
        self.vo.get_object(obj_data)
        global_obj,local_obj=self.vo.calc_vaild_obj([position_x,position_y,(heading)/180*pi])

        if not len(traffic_data) == 0:
            self.cc.checkObject(local_path,global_obj,local_obj,traffic_data[1],traffic_data[3])
        else:
            self.cc.checkObject(local_path,global_obj,local_obj,[],[])         

        #pure_pursuit. get_steering_angle_value                
        self.pure_pursuit.getPath(local_path) 
        self.pure_pursuit.getEgoStatus(position_x,position_y,position_z,velocity,heading)
            
        steering_angle=self.pure_pursuit.steering_angle()


        #ACC                 
        cc_vel = self.cc.acc(local_obj,velocity,self.vel_profile[current_waypoint]) ## advanced cruise control 적용한 속도 계획        
        print(self.vel_profile[current_waypoint])
        target_velocity = cc_vel
              

        control_input=self.pid.pid(target_velocity,velocity) ## 속도 제어를 위한 PID 적용 (target Velocity, Status Velocity)
        if control_input > 0 :
            accel= control_input
            brake= 0
        else :
            accel= 0
            brake= -control_input

        ctrl_mode = 2 # 2 = AutoMode / 1 = KeyBoard
        Gear = 4 # 4 1 : (P / parking ) 2 (R / reverse) 3 (N / Neutral)  4 : (D / Drive) 5 : (L)


        self.ctrl_cmd.send_data([ctrl_mode,Gear,accel,brake,steering_angle])     
        
        print('current_waypoint:{0}\tglobal_path_lenght:{1}'.format(self.current_waypoint,len(self.global_path)))
        print("sterring_angle:{0}\taccel:{1}\tbrake:{2}".format(steering_angle,accel,brake))
        
        
        if self.current_waypoint == len(self.global_path)-1: #Scenario끝나면 다시 Scenraio load           
            self.current_waypoint=0
            self.controller.commander(Command.SCEN_SETTING)