[ 충돌회피-2] 드론 리스폰&충돌감지&Unit Vector

충돌 혹은 도착점 도착후 새로운 시나리오가 시작될때 출발점으로 다시 가야함으로 아래와같은 코드를 작성했습니다.

1.리스폰 위치 알아내기

import airsim   

#setting for connecting Airsim 

client = airsim.MultirotorClient() 
client.confirmConnection() 
client.enableApiControl(True) 
client.armDisarm(True)  

def get_position(self):             
    # Get the vehicle pose (global position and orientation) of the drone             
    vehicle_pose = self.client.simGetVehiclePose()              
    # Get the global position (X, Y, Z) of the drone             
    global_position = vehicle_pose.position              
    # Print the global position             
    print("Drone's Global Position (X, Y, Z):")             
    print("X:", global_position.x_val)             
    print("Y:", global_position.y_val)             
    print("Z:", global_position.z_val)

2.리스폰 함수 작성

import airsim


client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

# 리스폰할 위치와 자세 설정
respawn_position = airsim.Vector3r(161.43, -7.745, 2.623)
respawn_orientation = airsim.to_quaternion(0, 0,90)  # (roll, pitch, yaw)

# 합성된 자세 쿼터니언 계산 합성곱 안해면 클남!!!!!
self.initial_orientation = airsim.to_quaternion(0, 0, 0)  # 초기 자세 (원점 자세)
final_orientation = initial_orientation * respawn_orientation

def drone_respone():
        
        collision_info=False
        client.simSetVehiclePose(airsim.Pose(respawn_position,final_orientation), True)

리스폰하고싶은 위치랑 원위치랑 쿼터니언을 합성곱해주지 않으면 계속 원점에 리스폰됨(내 컴퓨만 그런거 같기도…..)

json 파일을 수정하면 카메라 화면이 이상해집니다.

3.충돌 확인

simGetCollisionInfo()를 이용하여 충돌여부 확인 (자세한건 airsim API document에서 확인 가능)

def check_collision():

    info = self.client.simGetCollisionInfo()
    self.collision_info=info.has_collided
       
    if self.collision_info:
        for i in range(4):
            print("Collision occurred!")
        
        drone_respone()
       
    else:
        print("No collision")

4.Unit Vector

출발지점과 도착점 두점을 가지고 아래 같이 unit vector를 구할수있다.

 

def get_unit_vec(self):
        
        current_pose=np.array(self.get_position())
        print("current_pose",current_pose)
        destination=[95.29,18.09,0.88]
        
        vector=destination-current_pose
        unit_vector=vector/np.linalg.norm(vector)
        print("Unit Vector:",unit_vector)
        print("vector size:",np.linalg.norm(unit_vector))

코드는 좌표가 z축까지 3차원이지만 개인적으로 2차원을 추천한다(토이 프로젝트이기 때문에 굳이 힘뺄 필요가 없다.)