def __init__(self):
print("Connecting to Airsim Client:")
self.client = airsim.CarClient()
self.client.confirmConnection()
print("Connection confirmed. Enabling car controls...")
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
print("Car controls enabled.")
self.car_state = self.client.getCarState()
self.mode = True
"""
self.mode = input("Would you like the rover to repeat commands?(yes/no)"))
if self.mode == "yes":
self.mode = False
self.repeat = int(input("How many repeats?\n"))
if self.repeat < 0 or self.repeat > 30:
self.repeat = 2
"""
self.record = (input("Do you want to record images? (yes/no)"))
self.record = True if self.record == 'yes' else False
self.fold = os.getcwd()+'/pictures3/'
self.projectionMatrix = np.array([[1, 0.000000000, 0.000000000, -127.5000000000],
[0.000000000, 1, 0.000000000, -71.5000000000],
[0.000000000, 0.000000000, 1.00000000, 127.5 ],
[0.000000000, 0.000000000, -1/20.0000000, 0.000000000]])
color = (0, 255, 0)
self.rgb = "%d %d %d" % color
def __connect_to_airsim(self):
attempt_count = 0
while True:
try:
print('Attempting to connect to AirSim (attempt {0})'.format(
attempt_count))
self.__car_client = airsim.CarClient()
self.__car_client.confirmConnection()
self.__car_client.enableApiControl(True)
self.__car_controls = airsim.CarControls()
return
except:
print('Failed to connect.')
attempt_count += 1
if (attempt_count % 10 == 0):
print(
'10 consecutive failures to connect. Attempting to start AirSim on my own.'
)
if self.__local_run:
os.system('START "" powershell.exe {0}'.format(
os.path.join(
self.__airsim_path,
'AD_Cookbook_Start_AirSim.ps1 neighborhood -windowed'
)))
else:
os.system(
'START "" powershell.exe D:\\AD_Cookbook_AirSim\\Scripts\\DistributedRL\\restart_airsim_if_agent.ps1'
)
time.sleep(10)
def setSpeed():
pub = rospy.Publisher('sensor/speed', Float64, queue_size=1)
rospy.init_node('setspeed', anonymous=True)
rate = rospy.Rate(2) # 2 Hz
# connect to the AirSim simulator
client = airsim.CarClient()
client.confirmConnection()
client.enableApiControl(True)
car_controls = airsim.CarControls()
while not rospy.is_shutdown():
car_state = client.getCarState()
print("Speed %d, Gear %d" % (car_state.speed, car_state.gear))
# Get the speed of the car
car_controls.throttle = 1
car_controls.steering = 1
client.setCarControls(car_controls)
# let car drive a bit
time.sleep(1)
car_speed = client.getCarState().speed
rospy.loginfo(car_speed)
pub.publish(car_speed)
rate.sleep()
def __init__(self):
self.client = airsim.CarClient("10.8.105.156")
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.action_space = spaces.Discrete(7)
self.time_step = 0
self.x_pos_goal = 0.6 #0.545647
self.y_pos_goal = -2.5 #-1.419126
self.z_pos_goal = 0.2 #0.176768
self.counter_no_state = 0
self.w_rot_goal = 1.0 # 0.999967
self.x_rot_goal = 0.0 #
self.y_rot_goal = 0.0 # -0.000095
self.z_rot_goal = 0.02 # 0.019440
self.max_step = 10 # steps to check if blocked
self.last_states = queue.deque(maxlen=self.max_step)
self.height = 84
self.width = 84 # old 320
# self.observation_space = spaces.Box(low=-high, high=high, dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self.height, self.width, 2))
self.debug_mode = False
self.goal_counter = 0
self.count = 0
def __init__(self):
# connect to the AirSim simulator
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
def test_car_rand_pose(vehicle_client, car_client):
# get the fixed location to test the orientation randomness
starting_points_fixed, _ = get_random_pose()
i = 0
while i < 30:
# print('setting position')
starting_points, starting_direction = get_random_pose()
# set car location and orientation
vehicle_client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(starting_points[0], starting_points[1], starting_points[2]),
airsim.to_quaternion(starting_direction[0], starting_direction[1],
starting_direction[2])), True)
# test the car orientation
# print(starting_direction)
# car_client.simSetVehiclePose(
# airsim.Pose(airsim.Vector3r(starting_points_fixed[0], starting_points_fixed[1], starting_points_fixed[2]),
# airsim.to_quaternion(starting_direction[0], starting_direction[1],
# starting_direction[2] + 0.01)), True)
# print('wait for momentum die out')
car_controls = airsim.CarControls()
car_controls.steering = 0
car_controls.throttle = 0
car_controls.brake = 1
car_client.setCarControls(car_controls)
time.sleep(4)
i += 1
def step(self, action):
"""
Action should be a steer_dict = {"angle":float, "speed":float}
"""
car_controls = airsim.CarControls()
car_controls.throttle = action.get("speed")
car_controls.steering = action.get("angle")
self.client.setCarControls(car_controls)
# negspeed = action["speed"] * -1
# negangle = action["angle"] * -1
# action["speed"] = negspeed
# action["angle"] = negangle
# # Update Kinematics using Bicycle Model
# new_pose = self.car_state.update_state(action)
# self.client.simSetVehiclePose(new_pose, True)
time.sleep(0.001)
#get reward & check if done & return
obs = self._get_obs()
reward = self.get_reward()
done = self.tooclose()
info = {}
return obs, reward, done, info
def __init__(self):
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.device = "cuda"
self.reward_model = CNNNet().to(self.device)
self.reward_model.load_state_dict(
torch.load("model.dat", map_location=self.device))
self.reward_model.eval()
# input space.
high = np.array([np.inf, np.inf, 1., 1.])
low = np.array([-np.inf, -np.inf, 0., 0.])
self.observation_space = spaces.Box(low=low, high=high)
# action space: [steer, accel, brake]
self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(3, ))
self.default_action = [0.0, 1.0, 0.0]
# store vehicle speeds
self.max_speed = 3e5
self.prev_speed_sample = 40
self.past_vehicle_speeds = deque([self.max_speed] *
self.prev_speed_sample,
maxlen=self.prev_speed_sample)
self.done = False
self.lower_speed_limit = 5
def __init__(self):
print("Establishing connection to airsim!")
self.client= airsim.CarClient()
self.client.confirmConnection()
self.rover_state= self.client.getCarState()
print("Establishing Car controls")
self.client.enableApiControl(True)
self.rover_controls= airsim.CarControls()
print("Car Controls enabled")
# Include example json of this will look?
# rgb and depth will be in bytecode
# IMU:
imu_dict={
"speed":None,
"px":None,
"py": None,
"pz":None,
"ow":None,
"ox": None,
"oy": None,
"oz": None
}
self.data_dict={
"time": datetime.now().strftime("%Y-%m-%d %H:%M:%S"),
"rgb": None,
"depth": None,
"imu": imu_dict
}
def __connect_to_airsim(self):
attempt_count = 0
while True:
try:
print('Attempting to connect to AirSim (attempt {0})'.format(
attempt_count))
self.__car_client = airsim.CarClient() # CarClient()
self.__car_client.confirmConnection()
self.__car_client.enableApiControl(True)
self.__car_controls = airsim.CarControls()
print('Connected!')
return
except:
print('Failed to connect.')
attempt_count += 1
if (attempt_count % 10 == 0):
print(
'10 consecutive failures to connect. Attempting to start AirSim on my own.'
)
from subprocess import call
call(self.__airsim_path +
' -ResX=640 -ResY=480 -windowed &',
shell=True)
print('Waiting a few seconds.')
time.sleep(10)
def __init__(self):
self.client = airsim.CarClient("10.8.105.156")
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.action_space = spaces.Discrete(6)
self.time_step = 0
self.x_pos_goal_2 = 0.6 #0.545647 0.6
self.y_pos_goal_2 = -2.5 #-2.5
self.z_pos_goal_2 = 0.2 #0.2
self.w_rot_goal_2 = 1.0 # 0.999967
self.x_rot_goal_2 = 0.0 #
self.y_rot_goal_2 = 0.0 # -0.000095
self.z_rot_goal_2 = 0.02 # 0.019440
self.x_pos_goal_1 = 0.09 #0.545647 0.6
self.y_pos_goal_1 = -4.2 #-2.5
self.z_pos_goal_1 = 0.18 #0.2
self.w_rot_goal = 0.7 # 0.999967
self.x_rot_goal_1 = -0.7 #
self.y_rot_goal_1 = 0.0 # -0.000095
self.z_rot_goal_1 = -0.7 # 0.019440
self.task_one_complete = False
self.height = 84
self.width = 84 # old 320
# self.observation_space = spaces.Box(low=-high, high=high, dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(2, self.height, self.width))
self.debug_mode = False
self.goal_counter = 0
self.count = 0
self.state = None
def __init__(self):
print("Connecting to Airsim Client: ")
self.client = airsim.CarClient()
self.client.confirmConnection()
print("Connection confirmed, Enabling Car controls ")
self.client.enableApiControl(True)
self.car_controls= airsim.CarControls()
print("Car Controls enabled, Please enter Command parameters ")
self.car_state=self.client.getCarState()
self.mode = bool(input("Input car Mode: True (distance commands) " or "False"))
if(self.mode):
print("Going to Distance mode! ")
pass
else:
self.repeat= int(input("How many repeats? " or "3"))
self.record= bool(input("Record Images? " or "False"))
self.fold= os.getcwd()+'/pictures3/'
self.projectionMatrix = np.array([[1, 0.000000000, 0.000000000, -127.5000000000],
[0.000000000, 1, 0.000000000, -71.5000000000],
[0.000000000, 0.000000000, 1.00000000, 127.5 ],
[0.000000000, 0.000000000, -1/20.0000000, 0.000000000]])
color = (0,255,0)
self.rgb = "%d %d %d" % color
## Control Tello
self.drone = tellopy.Tello()
try:
self.drone.subscribe(drone.EVENT_FLIGHT_DATA,handler)
self.drone.connect()
drone.wait_for_connection(60.0)
def control(self, throttle=0, steering=0, brake=0, handbrake=False):
# 设置油门、方向和刹车
car_controls = airsim.CarControls(throttle=throttle,
steering=steering,
brake=brake,
handbrake=handbrake)
self.car_client.setCarControls(car_controls)
def init_car_controls():
car_controls = airsim.CarControls()
car_controls.throttle = 0.5125
car_controls.steering = 0.0
car_controls.is_manual_gear = True
car_controls.manual_gear = 1 # should constrain speed
return car_controls
def __init__(self, name='Car1'):
self.client = airsim.CarClient(port=41452)
self.name = name
self.init_client()
self.car_controls = airsim.CarControls()
self.current_pose = None
self.heading = None
atexit.register(self.disconnect)
def __init__(self, lidar_partition=60):
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.lidar_partition = lidar_partition # from left to right
self.command_count = 0
def send_control(steering_angle, throttle):
car_controls = airsim.CarControls()
car_controls.throttle = throttle
car_controls.steering = steering_angle
if throttle < 0.15:
car_controls.handbrake = True
else:
car_controls.handbrake = False
client.setCarControls(car_controls)
def setup():
cli = airsim.CarClient()
center = getPallet(cli)
car_spot = getCar(cli)
cli.enableApiControl(True)
car_controls = airsim.CarControls()
sleep(0.1)
return cli, car_controls, getPallet(cli), car_spot
def __send_command(self, command):
"""Sends driving commands over the AirSim API.
Args:
command (tuple): A tuple in the form (steering, throttle).
"""
car_control = airsim.CarControls()
car_control.steering = float(command[0])
car_control.throttle = command[1]
self.client.setCarControls(car_control)
def __init__(self):
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.initialCarControlSettings()
self.blocked_detection = 0
self.collision_detection = 0
self.reverse_count = 0
self.last_collision_object = None
def drive(self, car_name, action):
car_controls = airsim.CarControls()
car_controls.steering = action[0]
car_controls.throttle = action[1]
car_controls.brake = action[2]
if car_name == "Car1":
self.client.setCarControls(car_controls)
if car_name == "Car2":
self.client2.setCarControls(car_controls)
def __init__(self, distance, speed):
# set distance and speed for ACC algorithm
self.min_distance = distance
self.max_speed = speed
self.set_speed = speed
# connect to the AirSim simulator
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
return
def main():
global nmap, road_points, path_in_list, path_in_environment
# finish = turn.execute_turns(client, nmap, road_points, path_in_list, path_in_environment)
if len(sys.argv) == 2:
client = airsim.CarClient()
client.confirmConnection()
print('Connect succcefully！')
client.enableApiControl(True)
car_controls = airsim.CarControls()
client.simSetTimeOfDay(True, "2019-08-19 21:00:00", True, 1, 60, False)
client.simSetCameraOrientation(
2, airsim.to_quaternion(0, 0, -math.pi / 4))
client.simSetCameraOrientation(1,
airsim.to_quaternion(0, 0, math.pi / 4))
start_point = [13, 13]
target_point = [0, 22]
nmap, road_points, path_in_list, path_in_environment = turn.init_path_planning(
start_point, target_point)
if sys.argv[1] == 'test':
print('start testing')
testNetwork(client, car_controls, list(path_in_environment[-1]))
elif sys.argv[1] == 'train':
print('start training')
trainNetwork(client, car_controls)
else:
print('unkown input argument, please run this file as follow!')
print('python DQN_airsim.py train OR python DQN_airsim.py test')
elif len(sys.argv) == 3:
client = airsim.CarClient()
client.confirmConnection()
print('Connect succcefully！')
if sys.argv[1] == 'show_map' and sys.argv[2] == 'train':
print('display map')
show_map.show_map(client, [])
elif sys.argv[1] == 'show_map' and sys.argv[2] == 'test':
print('display map')
show_map.show_map(client, path_in_environment)
else:
print('unkown input argument, please run this file as follow!')
print(
'python DQN_airsim.py show_map train OR python DQN_airsim.py show_map test'
)
else:
print('please include an argument train or test or show_map')
def init_new_episode(self) :
self.last_action = [0,0]
self.last_reward = 0
self.client.enableApiControl(True, "AgentCar")
self.client.enableApiControl(True, "TargetCar")
self.car_controls = airsim.CarControls()
self.is_done = False
self.time_steps_left=self.max_time_steps
self.trajectory_distance=0
self.last_collision_depth=0
agent_car_pose = self.client.simGetVehiclePose(vehicle_name="TargetCar")
self.car_pos = [agent_car_pose.position.x_val, agent_car_pose.position.y_val, agent_car_pose.position.z_val]
self.update_current_state()
self.init_way_points()
def _act_discrete(self, action):
"""
Performs the discrete action
:param action: (int) action to perform
"""
self.car_state = self.client.getCarState()
current_speed = self.car_state.speed
throttle = 0.8 if current_speed <= self.max_speed else 0.0
steering = self.steering_angles[action]
self.client.setCarControls(
airsim.CarControls(throttle=throttle, steering=steering))
def runSingleCar(id: int):
client = airsim.CarClient()
client.confirmConnection()
vehicle_name = f"Car_{id}"
pose = airsim.Pose(airsim.Vector3r(0, 7 * id, 0),
airsim.Quaternionr(0, 0, 0, 0))
print(f"Creating {vehicle_name}")
success = client.simAddVehicle(vehicle_name, "Physxcar", pose)
if not success:
print(f"Falied to create {vehicle_name}")
return
# Sleep for some time to wait for other vehicles to be created
time.sleep(1)
# driveCar(vehicle_name, client)
print(f"Driving {vehicle_name} for a few secs...")
client.enableApiControl(True, vehicle_name)
car_controls = airsim.CarControls()
# go forward
car_controls.throttle = 0.5
car_controls.steering = 0
client.setCarControls(car_controls, vehicle_name)
time.sleep(3) # let car drive a bit
# Go forward + steer right
car_controls.throttle = 0.5
car_controls.steering = 1
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
# go reverse
car_controls.throttle = -0.5
car_controls.is_manual_gear = True
car_controls.manual_gear = -1
car_controls.steering = 0
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
car_controls.is_manual_gear = False # change back gear to auto
car_controls.manual_gear = 0
# apply brakes
car_controls.brake = 1
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
def __airSimClientConnection(self):
global client, clientCM, manualMode
self.__setClient(airsim.CarClient(timeout_value=10000))
self.__setClientCM(airsim.CarClient())
self.getClient().confirmConnection()
print("Do you want to activate the manual mode? [y/n]")
if input() == 'n':
manualMode = False
self.getClient().enableApiControl(True)
else:
manualMode = True
self.__setCarControls(airsim.CarControls())
time.sleep(2)
def __init__(self, show_cam=True):
# connect to the AirSim simulator
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.car_controls = airsim.CarControls()
self.episode_start = time.time()
self.SHOW_CAM = show_cam
self.fps = DEFAULT_FPS
self.period = 1 / DEFAULT_FPS
self.previous_action_time = None
self.step_num = 0
self.previous_distance = None
def _act_continuous(self, action):
"""
Performs the continious actions
TODO: add brake and parametrize map_to_range
:param action: (np.array) - actions to perform
"""
self.car_state = self.client.getCarState()
current_speed = self.car_state.speed
throttle = self._throttle_map(action[0])
throttle = 0.0 if current_speed >= self.max_speed else throttle
steering = self._steering_map(action[1])
self.client.setCarControls(
airsim.CarControls(throttle=throttle, steering=steering))
def __init__(self, dqn_param):
self.player_name = ""
self.car_controls = airsim.CarControls()
self.client = airsim.CarClient()
self.client.reset()
self.client.confirmConnection()
self.client.enableApiControl(enable_api_control, self.player_name)
self.airsim_env = AirSimEnv()
self.way_points, self.obstacle_points = self.airsim_env.load_track_info(
self.client)
self.collision_time_stamp = 0
self.sensing_info = CarState(self.player_name)
self.all_obstacles = self.airsim_env.get_all_obstacle_info(
self.obstacle_points, self.way_points)
self.state_size = self.airsim_env.get_state_size()
self.frozen_count = 0
self.car_current_pos_x, self.car_next_pos_x = 0, 0
# road half width + car half width
self.half_road_limit = self.client.getAlgoUserAPI(
).ac_road_width_half + 1.25
self.control_interval = round(0.1 / current_clock_speed, 2)
if len(self.action_space()) < 1:
raise IncorrectAction(
"Please check the action definition : At least one action is required"
)
self.action_size = len(self.action_space())
# DQN 에이전트 생성
self.agent = DQNAgent(self.state_size, self.action_size, dqn_param)
# running client id 로 폴더 생성
now = time.localtime()
self.run_cid = "T%02d%02d_%02d%02d%02d" % (
now.tm_mon, now.tm_mday, now.tm_hour, now.tm_min, now.tm_sec)
# 시간 생성.
self.start_time = time.time()
self.end_time = 0
