def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_transform, max_steer_angle, global_a, state0, global_collision
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.5)
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
# visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_img is not None) and (global_nav is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
# visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
# global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
if global_collision:
global_collision = False
start_point = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
start_waypoint = agent._map.get_waypoint(
agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_a = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
index = int((dt / args.max_t) // args.dt) + 2
if index > 0.99 / args.dt:
continue
"""
transform = vehicle.get_transform()
dx, dy, dyaw = get_transform(transform, global_transform)
dyaw = -dyaw
_x = global_trajectory['x'][index] - dx
_y = global_trajectory['y'][index] - dy
x = _x*np.cos(dyaw) + _y*np.sin(dyaw)
y = _y*np.cos(dyaw) - _x*np.sin(dyaw)
_vx = global_trajectory['vx'][index]
_vy = global_trajectory['vy'][index]
vx = _vx*np.cos(dyaw) + _vy*np.sin(dyaw)
vy = _vy*np.cos(dyaw) - _vx*np.sin(dyaw)
_ax = global_trajectory['ax'][index]
_ay = global_trajectory['ay'][index]
ax = _ax*np.cos(dyaw) + _ay*np.sin(dyaw)
ay = _ay*np.cos(dyaw) - _ax*np.sin(dyaw)
"""
#control = get_control(x, y, vx, vy, ax, ay)
control = ctrller.run_step(global_trajectory, index, state0)
vehicle.apply_control(control)
# x = global_trajectory['x']
# y = global_trajectory['y']
# plt.cla()
# plt.plot(x, y)
# plt.xlim(-1, 29)
# plt.ylim(-15, 15)
# plt.show()
"""
dyaw = np.deg2rad(global_transform.rotation.yaw)
x = global_trajectory['x'][index]*np.cos(dyaw) + global_trajectory['y'][index]*np.sin(dyaw)
y = global_trajectory['y'][index]*np.cos(dyaw) - global_trajectory['x'][index]*np.sin(dyaw)
"""
#localtion = carla.Location(x = global_transform.location.x+x, y=global_transform.location.y+y, z=2.0)
#world.debug.draw_point(localtion, size=0.3, color=carla.Color(255,0,0), life_time=10.0)
#print(global_vel*np.tan(control.steer)/w)
curve = None #show_traj(True)
visualize(global_view_img, global_nav, curve)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def main():
global global_nav, global_control, global_pos, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town02')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
70, 90))
#world.set_weather(carla.WeatherParameters.ClearNoon)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world,
blueprint,
vehicle_type='vehicle.bmw.grandtourer')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=1.5, y=0.0, z=2.0)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
FPS = [str(time.time())]
for cnt in range(args.num):
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
#control = agent.run_step()
#control.manual_gear_shift = False
#control = None
#vehicle.apply_control(control)
#nav = get_nav(vehicle, plan_map)
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
start_waypoint = agent._map.get_waypoint(agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
x = []
y = []
traj_pose_list = []
for i in range(len(route_trace) - 1):
waypoint1 = route_trace[i][0].transform.location
waypoint2 = route_trace[i + 1][0].transform.location
dist = waypoint1.distance(waypoint2)
x.append(waypoint1.x)
y.append(waypoint1.y)
traj_pose_list.append((0, route_trace[i][0].transform))
plt.plot([global_pos.location.x, x[0]], [global_pos.location.y, y[0]],
color='r')
plt.plot(x[:10], y[:10], color='b')
plt.show()
empty_image = collect_perspective.getPM(traj_pose_list, global_pos,
global_img)
break
#cv2.imshow('Nav', nav)
#cv2.imshow('Vision', global_img)
#cv2.waitKey(10)
index = str(time.time())
if cnt % 100 == 0:
FPS.append(index)
print(round(1 / ((float(FPS[-1]) - float(FPS[-2])) / 100), 1))
cv2.destroyAllWindows()
vehicle.destroy()
sm.close_all()
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=9.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
ctrller = CapacController(world, vehicle, 30)
agent = BasicAgent(vehicle, target_speed=30)
world_map = world.get_map()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
spawn_points = world_map.get_spawn_points()
route_trace = None
start_point = random.choice(spawn_points)
destination = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
global_dict['collision'] = False
start_waypoint = agent._map.get_waypoint(agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
plan_thread = threading.Thread(target=make_plan, args=())
while True:
if (global_dict['img'] is not None) and (global_dict['nav']
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
while global_dict['trajectory'] is None:
time.sleep(0.001)
while True:
done = False
trajectory = global_dict['trajectory']
for i in range(20):
if global_dict['collision']:
done = True
print('Done !')
break
if close2dest(vehicle, destination):
done = True
print('Success !')
break
control_time = time.time()
dt = control_time - trajectory['time']
index = int((dt / args.max_t) // args.dt) + 5
if index > 0.99 // args.dt - 10:
continue
control = ctrller.run_step(trajectory, index,
global_dict['state0'])
vehicle.apply_control(control)
cv2.imshow('Visualization', global_dict['view_img'])
cv2.imshow('Costmap', global_dict['draw_cost_map'])
cv2.waitKey(1)
if done or global_dict['collision']:
start_point = random.choice(spawn_points)
destination = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
global_dict['collision'] = False
start_waypoint = agent._map.get_waypoint(
agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
done = False
sm.close_all()
vehicle.destroy()
class CARLAEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': 60
}
def __init__(self, world, vehicle, global_dict, args, trajectory_model):
self.world = world
self.vehicle = vehicle
self.agent = BasicAgent(self.vehicle, target_speed=30)
self.global_dict = global_dict
self.args = args
self.trajectory_model = trajectory_model
self.ctrller = CapacController(self.world, self.vehicle, 30)
# robot action space
self.low_action = np.array([-1, -1])
self.high_action = np.array([1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
# robot observation space
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(7, ),
dtype=np.float32)
world_map = self.world.get_map()
waypoint_tuple_list = world_map.get_topology()
self.origin_map = get_map(waypoint_tuple_list)
self.spawn_points = world_map.get_spawn_points()
self.route_trace = None
# environment feedback infomation
self.state = []
self.done = False
self.reward = 0.0
self.seed()
self.reset()
#def step(self, action=[0,0]):
def step(self, action, plan_time):
t = torch.arange(0, 0.99, self.args.dt).unsqueeze(1).to(device)
t.requires_grad = True
v_0 = torch.FloatTensor([self.global_dict['v0'] / self.args.max_speed
]).expand(len(t), 1)
v_0 = v_0.to(device)
t_with_v = torch.cat([t, v_0], dim=1)
noise = torch.FloatTensor(action).unsqueeze(0).unsqueeze(0)
noise = noise.expand(1, len(t), self.args.vector_dim)
noise = noise.reshape(1 * len(t), self.args.vector_dim)
noise.requires_grad = True
noise = noise.to(device)
output_xy = self.trajectory_model(noise, t_with_v)
#print("output_xy:", output_xy.shape)
vx = grad(output_xy[:, 0].sum(), t, create_graph=True)[0][:, 0] * (
self.args.max_dist / self.args.max_t)
vy = grad(output_xy[:, 1].sum(), t, create_graph=True)[0][:, 0] * (
self.args.max_dist / self.args.max_t)
ax = grad(vx.sum(), t, create_graph=True)[0][:, 0] / self.args.max_t
ay = grad(vy.sum(), t, create_graph=True)[0][:, 0] / self.args.max_t
output_axy = torch.cat([ax.unsqueeze(1), ay.unsqueeze(1)], dim=1)
x = output_xy[:, 0] * self.args.max_dist
y = output_xy[:, 1] * self.args.max_dist
theta_a = torch.atan2(ay, ax)
theta_v = torch.atan2(vy, vx)
sign = torch.sign(torch.cos(theta_a - theta_v))
a = torch.mul(torch.norm(output_axy, dim=1),
sign.flatten()).unsqueeze(1)
# draw
#self.global_dict['draw_cost_map'] = draw_traj(self.global_dict['cost_map'], output_xy, self.args)
vx = vx.data.cpu().numpy()
vy = vy.data.cpu().numpy()
x = x.data.cpu().numpy()
y = y.data.cpu().numpy()
ax = ax.data.cpu().numpy()
ay = ay.data.cpu().numpy()
a = a.data.cpu().numpy()
trajectory = {
'time': plan_time,
'x': x,
'y': y,
'vx': vx,
'vy': vy,
'ax': ax,
'ay': ay,
'a': a
}
self.reward = 0.
waypoint1, index1, diff_deg1 = self.find_waypoint()
waypoint = carla.Location(x=waypoint1.location.x,
y=waypoint1.location.y,
z=2.0)
self.world.debug.draw_point(waypoint,
size=0.2,
color=carla.Color(255, 0, 0),
life_time=1.5)
for i in range(20):
if self.global_dict['collision']:
self.done = True
self.reward -= 500.
print('Done !')
break
if close2dest(self.vehicle, self.destination):
self.done = True
#self.reward += 1000.
print('Success !')
break
"""
for i in range(len(self.route_trace)):
x = self.route_trace[i][0].transform.location.x
y = self.route_trace[i][0].transform.location.y
localtion = carla.Location(x=x, y=y, z=2.0)
self.world.debug.draw_point(localtion, size=0.2, color=carla.Color(255,0,0), life_time=0.5)
"""
if i % 5 == 0:
self.global_dict['draw_cost_map'] = draw_traj(
self.global_dict['cost_map'], output_xy, self.args)
visualize(self.global_dict,
self.global_dict['draw_cost_map'],
self.args,
curve=None)
control_time = time.time()
dt = control_time - trajectory['time']
index = int((dt / self.args.max_t) // self.args.dt) + 5
if index > 0.99 // self.args.dt - 10:
continue
control = self.ctrller.run_step(trajectory, index,
self.global_dict['state0'])
self.vehicle.apply_control(control)
time.sleep(0.005)
trans_costmap_stack = get_costmap_stack(self.global_dict)
trans_costmap_stack = torch.stack(trans_costmap_stack)
self.state = trans_costmap_stack
waypoint2, index2, diff_deg2 = self.find_waypoint()
#print(index2, index1)
if index2 > index1:
self.reward += 1.
else:
self.reward -= 1.
if diff_deg2 <= diff_deg1:
self.reward += (180. - diff_deg2) / 180.
else:
self.reward -= 2 * diff_deg2 / 180.
self.reward += 2
return self.state, self.reward, self.done, {}
def find_waypoint(self):
position = self.vehicle.get_transform().location
yaw = self.vehicle.get_transform().rotation.yaw
asb_dists = []
for i in range(len(self.route_trace)):
location = self.route_trace[i][0].transform.location
x = location.x
y = location.y
asb_dists.append(np.sqrt((x - position.x)**2 +
(y - position.y)**2))
#waypoint = carla.Location(x=x, y=y, z=2.0)
#self.world.debug.draw_point(localtion, size=0.2, color=carla.Color(255,0,0), life_time=0.5)
min_val = min(asb_dists)
index = asb_dists.index(min_val)
if min_val < 2.:
index += 10
index = np.clip(index, 0, len(self.route_trace) - 1)
try:
x0 = self.route_trace[index][0].transform.location.x
y0 = self.route_trace[index][0].transform.location.y
x1 = self.route_trace[index + 1][0].transform.location.x
y1 = self.route_trace[index + 1][0].transform.location.y
wp_yaw = np.arctan2(y1 - y0, x1 - x0)
#wp_yaw = np.rad2deg(wp_yaw)
except:
wp_yaw = 0.0
#print('11111', self.route_trace[index][0].transform.rotation.yaw, yaw)
#print('vehicle:', yaw)
yaw = np.deg2rad(yaw)
#print('ddddddddd', np.rad2deg(abs(wp_yaw-yaw)))
#print(wp_yaw, yaw)
return self.route_trace[index][0].transform, index, np.rad2deg(
abs(wp_yaw - yaw))
def reset(self):
start_point = random.choice(self.spawn_points)
self.destination = random.choice(self.spawn_points)
self.vehicle.set_transform(start_point)
self.global_dict['plan_map'] = replan(self.agent, self.destination,
copy.deepcopy(self.origin_map))
self.global_dict['collision'] = False
start_waypoint = self.agent._map.get_waypoint(
self.agent._vehicle.get_location())
end_waypoint = self.agent._map.get_waypoint(self.destination.location)
self.route_trace = self.agent._trace_route(start_waypoint,
end_waypoint)
start_point.rotation = self.route_trace[0][0].transform.rotation
self.vehicle.set_transform(start_point)
#time.sleep(0.1)
trans_costmap_stack = get_costmap_stack(self.global_dict)
trans_costmap_stack = torch.stack(trans_costmap_stack)
self.state = trans_costmap_stack[0]
self.done = False
self.reward = 0.0
print('RESET !!!!!!!!')
return self.state
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _angle_normalize(self, angle):
if angle > np.pi:
angle -= 2 * np.pi
elif angle < -np.pi:
angle += 2 * np.pi
return angle
class CARLAEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': 60
}
def __init__(self, world, vehicle, global_dict, args, trajectory_model):
self.world = world
self.vehicle = vehicle
self.agent = BasicAgent(self.vehicle, target_speed=30)
self.global_dict = global_dict
self.args = args
self.trajectory_model = trajectory_model
self.ctrller = CapacController(self.world, self.vehicle, 50)
# robot action space
self.low_action = np.array([-1, -1])
self.high_action = np.array([1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
# robot observation space
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(7, ),
dtype=np.float32)
world_map = self.world.get_map()
waypoint_tuple_list = world_map.get_topology()
self.origin_map = get_map(waypoint_tuple_list)
self.spawn_points = world_map.get_spawn_points()
self.route_trace = None
# environment feedback infomation
self.state = []
self.done = False
self.reward = 0.0
self.seed()
self.reset()
#def step(self, action=[0,0]):
def step(self, action, plan_time):
t = torch.arange(0, 0.99, self.args.dt).unsqueeze(1).to(device)
t.requires_grad = True
self.global_dict['v0'] = max(3.0, self.global_dict['v0'])
v_0 = torch.FloatTensor([self.global_dict['v0'] / self.args.max_speed
]).expand(len(t), 1)
v_0 = v_0.to(device)
# t_with_v = torch.cat([t, v_0], dim=1)
noise = torch.FloatTensor(action).unsqueeze(0).unsqueeze(0)
noise = noise.expand(1, len(t), self.args.vector_dim)
noise = noise.reshape(1 * len(t), self.args.vector_dim)
#noise.requires_grad = True
noise = noise.to(device)
# print("noise shape:", noise.shape)
# print("t_with_v shape:", t_with_v.shape)
# print("t shape:", t.shape)
# print("v0 shape:", v_0.shape)
# output_xy = self.trajectory_model(noise, t_with_v, t)
output_xy = self.trajectory_model(v_0, noise, t)
#print("output_xy:", output_xy.shape)
# print(output_xy[:,0].sum())
vx = grad(output_xy[:, 0].sum(), t, create_graph=True)[0][:, 0] * (
self.args.max_dist / self.args.max_t)
vy = grad(output_xy[:, 1].sum(), t, create_graph=True)[0][:, 0] * (
self.args.max_dist / self.args.max_t)
ax = grad(vx.sum(), t, create_graph=True)[0][:, 0] / self.args.max_t
ay = grad(vy.sum(), t, create_graph=True)[0][:, 0] / self.args.max_t
output_axy = torch.cat([ax.unsqueeze(1), ay.unsqueeze(1)], dim=1)
x = output_xy[:, 0] * self.args.max_dist
y = output_xy[:, 1] * self.args.max_dist
theta_a = torch.atan2(ay, ax)
theta_v = torch.atan2(vy, vx)
sign = torch.sign(torch.cos(theta_a - theta_v))
a = torch.mul(torch.norm(output_axy, dim=1),
sign.flatten()).unsqueeze(1)
# draw
#self.global_dict['draw_cost_map'] = draw_traj(self.global_dict['cost_map'], output_xy, self.args)
vx = vx.data.cpu().numpy()
vy = vy.data.cpu().numpy()
x = x.data.cpu().numpy()
y = y.data.cpu().numpy()
ax = ax.data.cpu().numpy()
ay = ay.data.cpu().numpy()
a = a.data.cpu().numpy()
trajectory = {
'time': plan_time,
'x': x,
'y': y,
'vx': vx,
'vy': vy,
'ax': ax,
'ay': ay,
'a': a
}
self.reward = 0.
waypoint1, index1, diff_deg1 = self.find_waypoint()
waypoint = carla.Location(x=waypoint1.location.x,
y=waypoint1.location.y,
z=2.0)
self.world.debug.draw_point(waypoint,
size=0.2,
color=carla.Color(255, 0, 0),
life_time=1.0)
old_loc_x = self.vehicle.get_transform().location.x
old_loc_y = self.vehicle.get_transform().location.y
org_dist = np.sqrt((self.vehicle.get_transform().location.x -
waypoint1.location.x)**2 +
(self.vehicle.get_transform().location.y -
waypoint1.location.y)**2)
vehicle2point = np.arctan2(
waypoint1.location.y - self.vehicle.get_transform().location.y,
waypoint1.location.x - self.vehicle.get_transform().location.x)
vehicle_yaw = np.deg2rad(self.vehicle.get_transform().rotation.yaw)
delta_theta = abs(self._angle_normalize(vehicle2point - vehicle_yaw))
# error = -org_dist * np.sin(delta_theta)
"""
for i in range(index1, min(index1+10, len(self.route_trace)-1)):
location = self.route_trace[i][0].transform.location
x = location.x
y = location.y
waypoint = carla.Location(x=x, y=y, z=2.0)
self.world.debug.draw_point(waypoint, size=0.2, color=carla.Color(255,0,0), life_time=0.5)
"""
for i in range(50):
if self.global_dict['collision']:
self.done = True
self.reward -= 50. #-50
print('Done !')
break
if close2dest(self.vehicle, self.destination):
self.done = True
self.reward += 100.
print('Success !')
break
"""
for i in range(len(self.route_trace)):
x = self.route_trace[i][0].transform.location.x
y = self.route_trace[i][0].transform.location.y
localtion = carla.Location(x=x, y=y, z=2.0)
self.world.debug.draw_point(localtion, size=0.2, color=carla.Color(255,0,0), life_time=0.5)
"""
if self.args.show and i % 5 == 0:
self.global_dict['draw_cost_map'] = draw_traj(
self.global_dict['cost_map'], output_xy, self.args)
visualize(self.global_dict,
self.global_dict['draw_cost_map'],
self.args,
curve=None)
control_time = time.time()
dt = control_time - trajectory['time']
index = int((dt / self.args.max_t) // self.args.dt) + 8
if index > 0.99 // self.args.dt - 10:
continue
control = self.ctrller.run_step(trajectory, index,
self.global_dict['state0'])
self.vehicle.apply_control(control)
time.sleep(0.01)
"""
trans_costmap_stack = get_costmap_stack(self.global_dict)
trans_costmap_stack = torch.stack(trans_costmap_stack)
self.state = trans_costmap_stack
"""
new_dist = np.sqrt((self.vehicle.get_transform().location.x -
waypoint1.location.x)**2 +
(self.vehicle.get_transform().location.y -
waypoint1.location.y)**2)
vehicle2point_new = np.arctan2(
waypoint1.location.y - self.vehicle.get_transform().location.y,
waypoint1.location.x - self.vehicle.get_transform().location.x)
vehicle_yaw_new = np.deg2rad(self.vehicle.get_transform().rotation.yaw)
delta_thet_new = abs(
self._angle_normalize(vehicle2point_new - vehicle_yaw_new))
# error_new = -new_dist * np.sin(delta_thet_new)
dist_reward = (org_dist - new_dist) #/10.0
# theta_reward = (delta_theta - delta_thet_new) # * 60 add by yujiyu
# theta_reward = (error - error_new)*100
step_dist = np.sqrt(
(self.vehicle.get_transform().location.x - old_loc_x)**2 +
(self.vehicle.get_transform().location.y - old_loc_y)**2)
diff_deg1 = abs(
np.rad2deg(self._angle_normalize(np.deg2rad(diff_deg1))))
if diff_deg1 < 10:
theta_reward = (10 - diff_deg1) / 3
else:
theta_reward = -diff_deg1 / 10.
theta_reward = np.clip(theta_reward, -3, 3)
# self.reward += dist_reward
self.reward += theta_reward
self.reward += step_dist * 2.
v = self.vehicle.get_velocity()
v0 = np.sqrt(v.x**2 + v.y**2 + v.z**2) / 2. #/4
self.reward += v0
print("v:%.2f theta:%.2f dist:%.2f " % (v0, theta_reward, step_dist))
self.state = copy.deepcopy(self.global_dict['trans_costmap'])
#self.global_dict['ts']
#waypoint2, index2, diff_deg2 = self.find_waypoint()
#print(index2, index1)
return self.state, self.reward, self.done, copy.deepcopy(
self.global_dict['ts'])
def find_waypoint(self):
position = self.vehicle.get_transform().location
yaw = self.vehicle.get_transform().rotation.yaw
asb_dists = []
for i in range(len(self.route_trace)):
location = self.route_trace[i][0].transform.location
x = location.x
y = location.y
asb_dists.append(np.sqrt((x - position.x)**2 +
(y - position.y)**2))
#waypoint = carla.Location(x=x, y=y, z=2.0)
#self.world.debug.draw_point(waypoint, size=0.2, color=carla.Color(255,0,0), life_time=0.5)
min_val = min(asb_dists)
index = asb_dists.index(min_val)
#if min_val < 1.0:
index += 5
index = np.clip(index, 0, len(self.route_trace) - 1)
try:
x0 = self.route_trace[index][0].transform.location.x
y0 = self.route_trace[index][0].transform.location.y
x1 = self.route_trace[index + 1][0].transform.location.x
y1 = self.route_trace[index + 1][0].transform.location.y
wp_yaw = np.arctan2(y1 - y0, x1 - x0)
#wp_yaw = np.rad2deg(wp_yaw)
except:
wp_yaw = 0.0
#print('11111', self.route_trace[index][0].transform.rotation.yaw, yaw)
#print('vehicle:', yaw)
yaw = np.deg2rad(yaw)
#print('ddddddddd', np.rad2deg(abs(wp_yaw-yaw)))
#print(wp_yaw, yaw)
return self.route_trace[index][0].transform, index, np.rad2deg(
abs(wp_yaw - yaw))
def reset(self):
# start_point = random.choice(self.spawn_points)
# yujiyu
start_point = self.spawn_points[2]
self.destination = random.choice(self.spawn_points)
self.vehicle.set_transform(start_point)
self.global_dict['plan_map'], self.destination = replan(
self.agent, self.destination, copy.deepcopy(self.origin_map),
self.spawn_points)
self.global_dict['collision'] = False
start_waypoint = self.agent._map.get_waypoint(
self.agent._vehicle.get_location())
end_waypoint = self.agent._map.get_waypoint(self.destination.location)
self.route_trace = self.agent._trace_route(start_waypoint,
end_waypoint)
start_point.rotation = self.route_trace[0][0].transform.rotation
self.vehicle.set_transform(start_point)
#yujiyu
self.vehicle.apply_control(
carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0))
#
"""
trans_costmap_stack = get_costmap_stack(self.global_dict)
trans_costmap_stack = torch.stack(trans_costmap_stack)
self.state = trans_costmap_stack[0]
"""
self.state = self.global_dict['trans_costmap']
self.done = False
self.reward = 0.0
# print('RESET !!!!!!!!')
return self.state
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _angle_normalize(self, angle):
if angle > np.pi:
angle -= 2 * np.pi
elif angle < -np.pi:
angle += 2 * np.pi
return angle
class NpcAgent(AutonomousAgent):
"""
NPC autonomous agent to control the ego vehicle
"""
_agent = None
_route_assigned = False
def setup(self, path_to_conf_file):
"""
Setup the agent parameters
"""
self.track = Track.SENSORS
self._route_assigned = False
self._agent = None
def sensors(self):
"""
Define the sensor suite required by the agent
:return: a list containing the required sensors in the following format:
[
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Left'},
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Right'},
{'type': 'sensor.lidar.ray_cast', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0,
'id': 'LIDAR'}
"""
sensors = [
{
'type': 'sensor.camera.rgb',
'x': 0.7,
'y': -0.4,
'z': 1.60,
'roll': 0.0,
'pitch': 0.0,
'yaw': 0.0,
'width': 300,
'height': 200,
'fov': 100,
'id': 'Left'
},
]
return sensors
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
hero_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == 'hero':
hero_actor = actor
break
if hero_actor:
self._agent = BasicAgent(hero_actor)
return control
if not self._route_assigned:
if self._global_plan:
plan = []
prev = None
for transform, _ in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(
transform.location)
if prev:
route_segment = self._agent._trace_route(prev, wp)
plan.extend(route_segment)
prev = wp
#loc = plan[-1][0].transform.location
#self._agent.set_destination([loc.x, loc.y, loc.z])
self._agent._local_planner.set_global_plan(plan) # pylint: disable=protected-access
self._route_assigned = True
else:
control = self._agent.run_step()
return control