| 34 | subprocess.run(['kill', '-9', cr_pid]) |
| 35 | |
| 36 | class AirsimBridge: |
| 37 | def __init__(self, env_name): |
| 38 | self.env_name = env_name |
| 39 | self._sim_thread = threading.Thread(target=self._init_airsim_sim) |
| 40 | self._sim_thread.start() |
| 41 | time.sleep(10) |
| 42 | |
| 43 | self._client = airsim.MultirotorClient() |
| 44 | self._client.confirmConnection() |
| 45 | self._client.enableApiControl(True) |
| 46 | self._client.armDisarm(True) |
| 47 | |
| 48 | self.distance_to_goal = [] |
| 49 | self.spl = [] |
| 50 | self.success = [] |
| 51 | self.traj_len = 0 |
| 52 | self.pass_len = 1e-3 |
| 53 | self.osr = [] |
| 54 | |
| 55 | def _init_airsim_sim(self): |
| 56 | env_dir = "envs/airsim/" + self.env_name |
| 57 | |
| 58 | if not os.path.exists(env_dir): |
| 59 | raise ValueError(f"Specified directory {env_dir} does not exist") |
| 60 | |
| 61 | command = ["bash", f"{env_dir}/LinuxNoEditor/start.sh"] |
| 62 | self.process = subprocess.Popen(command, text=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) |
| 63 | stdout, stderr = self.process.communicate() |
| 64 | # print("Command output:\n", stdout) |
| 65 | |
| 66 | def print_info(self): |
| 67 | print(f"SR: {self.success[-1]}, OSR: {self.osr[-1]}, NE: {self.distance_to_goal[-1]}, SPL: {self.spl[-1]}") |
| 68 | return f"SR: {self.success[-1]}, OSR: {self.osr[-1]}, NE: {self.distance_to_goal[-1]}, SPL: {self.spl[-1]}" |
| 69 | def set_camera_pose(self, x, y, z, pitch, yaw, roll): |
| 70 | target_pose = airsim.Pose(airsim.Vector3r(x, -y, -z), |
| 71 | airsim.to_quaternion(math.radians(pitch), 0, math.radians(-yaw))) |
| 72 | self._client.moveByVelocityBodyFrameAsync(0, 0, 0, 0.02) |
| 73 | self._client.simSetVehiclePose(target_pose, True) |
| 74 | |
| 75 | def set_drone_pos(self, x, y, z, pitch, yaw, roll): |
| 76 | self._client.moveByVelocityBodyFrameAsync(0, 0, 0, 0.02) |
| 77 | qua = euler_to_quaternion(pitch, -yaw, roll) |
| 78 | target_pose = airsim.Pose(airsim.Vector3r(x, y, z), |
| 79 | airsim.Quaternionr(qua[0], qua[1], qua[2], qua[3])) |
| 80 | self._client.simSetVehiclePose(target_pose, True) |
| 81 | self._client.moveByVelocityBodyFrameAsync(0, 0, 0, 0.02) |
| 82 | time.sleep(0.1) |
| 83 | |
| 84 | def _camera_init(self): |
| 85 | '''Camera initialization''' |
| 86 | camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(math.radians(15), 0, 0)) |
| 87 | self._client.simSetCameraPose("0", camera_pose) |
| 88 | time.sleep(1) |
| 89 | |
| 90 | def _drone_init(self): |
| 91 | '''Drone initialization''' |
| 92 | self.set_drone_pos(0, 0, 0, 0, 0, 0) |
| 93 | time.sleep(1) |