diff --git a/src/distributed_hierarchical_attentive/.gitignore b/src/distributed_hierarchical_attentive/.gitignore new file mode 100644 index 0000000000..02c4538dbe --- /dev/null +++ b/src/distributed_hierarchical_attentive/.gitignore @@ -0,0 +1,3 @@ +*out/ +*__pycache__/ +*.idea/ \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/LICENSE b/src/distributed_hierarchical_attentive/LICENSE new file mode 100644 index 0000000000..261eeb9e9f --- /dev/null +++ b/src/distributed_hierarchical_attentive/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/distributed_hierarchical_attentive/algs/pdqn.py b/src/distributed_hierarchical_attentive/algs/pdqn.py new file mode 100644 index 0000000000..6b1eaec3cd --- /dev/null +++ b/src/distributed_hierarchical_attentive/algs/pdqn.py @@ -0,0 +1,643 @@ +import random, collections +import numpy as np +import torch, logging +from torch import nn +import torch.nn.functional as F +from torch.autograd import Variable +from algs.util.replay_buffer import SumTree, SplitReplayBuffer + + +# + +class PriReplayBuffer(object): # stored as ( s, a, r, s_ ) in SumTree + """ + Prioritized experience replay + This Memory class is modified based on the original code from: + https://github.com/jaara/AI-blog/blob/master/Seaquest-DDQN-PER.py + Detailed information: + https://yulizi123.github.io/tutorials/machine-learning/reinforcement-learning/4-6-prioritized-replay/ + """ + epsilon = 0.01 # small amount to avoid zero priority + alpha = 0.6 # [0~1] convert the importance of TD error to priority. If alpha = 0, there is no Importance Sampling. + beta = 0.4 # importance-sampling, from initial value increasing to 1 + beta_increment_per_sampling = 0.001 + abs_err_upper = 1. # clipped abs error + + def __init__(self, capacity): + self.tree = SumTree(capacity) + + def add(self, transition): + max_p = np.max(self.tree.tree[-self.tree.capacity:]) + if max_p == 0: + max_p = self.abs_err_upper + self.tree.add(max_p, transition) # set the max p for new p + + def sample(self, n): + # assert self.tree.size==self.tree.capacity + b_idx, ISWeights = np.empty((n,), dtype=np.int32), np.empty((n, 1)) + b_state, b_action, b_action_param, b_reward, b_next_state, b_truncated, b_done, b_info = [], [], [], [], [], [], [], [] + pri_seg = self.tree.total_p / n # priority segment + self.beta = np.min([1., self.beta + self.beta_increment_per_sampling]) # max = 1 + + min_prob = np.min( + self.tree.tree[self.tree.capacity - 1:self.tree.capacity - 1 + self.size()]) / self.tree.total_p + # min_prob = np.min(self.tree.tree[-self.tree.capacity:]) / self.tree.total_p # for later calculate ISweight + for i in range(n): + a, b = pri_seg * i, pri_seg * (i + 1) + v = np.random.uniform(a, b) # sample from [a, b) + idx, p, data = self.tree.get_leaf(v) + prob = p / self.tree.total_p + ISWeights[i, 0] = np.power(prob / min_prob, -self.beta) + b_idx[i] = idx + b_state.append(data[0]) + b_action.append(data[1]) + b_action_param.append(data[2]) + b_reward.append(data[3]) + b_next_state.append(data[4]) + b_truncated.append(data[5]) + b_done.append(data[6]) + b_info.append(data[7]) + + # print(self.tree.tree) + # print(b_idx) + return b_idx, ISWeights, (b_state, b_action, b_action_param, b_reward, b_next_state, b_truncated, b_done, + b_info) + + def batch_update(self, tree_idx, abs_errors): + abs_errors += self.epsilon # convert to abs and avoid 0 + clipped_errors = np.minimum(abs_errors, self.abs_err_upper) + ps = np.power(clipped_errors, self.alpha) + for ti, p in zip(tree_idx, ps): + self.tree.update(ti, p) + + def size(self): + return self.tree.size + + +class veh_lane_encoder(torch.nn.Module): + def __init__(self, state_dim, train=True): + super().__init__() + self.state_dim = state_dim + self.train = train + self.lane_encoder = nn.Linear(state_dim['waypoints'], 32) + self.veh_encoder = nn.Linear(state_dim['companion_vehicle'] * 2, 64) + self.light_encoder = nn.Linear(state_dim['light'], 32) + self.agg = nn.Linear(128, 64) + + def forward(self, lane_veh, ego_info): + lane = lane_veh[:, :self.state_dim["waypoints"]] + veh = lane_veh[:, self.state_dim["waypoints"]:-self.state_dim['light']] + light = lane_veh[:, -self.state_dim['light']:] + lane_enc = F.relu(self.lane_encoder(lane)) + veh_enc = F.relu(self.veh_encoder(veh)) + light_enc = F.relu(self.light_encoder(light)) + state_cat = torch.cat((lane_enc, veh_enc, light_enc), dim=1) + state_enc = F.relu(self.agg(state_cat)) + return state_enc + + +class lane_wise_cross_attention_encoder(torch.nn.Module): + def __init__(self, state_dim, train=True): + super().__init__() + self.state_dim = state_dim + self.train = train + self.hidden_size = 64 + self.lane_encoder = nn.Linear(state_dim['waypoints'], self.hidden_size) + self.veh_encoder = nn.Linear(state_dim['companion_vehicle'] * 2, self.hidden_size) + self.light_encoder = nn.Linear(state_dim['light'], self.hidden_size) + self.ego_encoder = nn.Linear(state_dim['ego_vehicle'], self.hidden_size) + self.w = nn.Linear(self.hidden_size, self.hidden_size) + self.ego_a = nn.Linear(self.hidden_size, 1) + self.ego_o = nn.Linear(self.hidden_size, 1) + self.leaky_relu = nn.LeakyReLU(negative_slope=0.1) + + def forward(self, lane_veh, ego_info): + batch_size = lane_veh.shape[0] + lane = lane_veh[:, :self.state_dim["waypoints"]] + veh = lane_veh[:, self.state_dim["waypoints"]:-self.state_dim['light']] + light = lane_veh[:, -self.state_dim['light']:] + # print('ego_info.shape: ', ego_info.shape) + ego_enc = self.w(F.relu(self.ego_encoder(ego_info))) + lane_enc = self.w(F.relu(self.lane_encoder(lane))) + veh_enc = self.w(F.relu(self.veh_encoder(veh))) + light_enc = self.w(F.relu(self.light_encoder(light))) + state_enc = torch.cat((lane_enc, veh_enc, light_enc), 1).reshape(batch_size, 3, self.hidden_size) + # _enc: [batch_size, 32] + score_lane = self.leaky_relu(self.ego_a(ego_enc) + self.ego_o(lane_enc)) + score_veh = self.leaky_relu(self.ego_a(ego_enc) + self.ego_o(veh_enc)) + score_light = self.leaky_relu(self.ego_a(ego_enc) + self.ego_o(light_enc)) + # score_: [batch_size, 1] + score = torch.cat((score_lane, score_veh, score_light), 1) + score = F.softmax(score, 1).reshape(batch_size, 1, 3) + state_enc = torch.matmul(score, state_enc).reshape(batch_size, self.hidden_size) + # state_enc: [N, 64] + return state_enc + + +class PolicyNet_multi(torch.nn.Module): + def __init__(self, state_dim, action_parameter_size, action_bound, train=True) -> None: + # the action bound and state_dim here are dicts + super().__init__() + self.state_dim = state_dim + self.action_bound = action_bound + self.action_parameter_size = action_parameter_size + self.train = train + self.left_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.center_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.right_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.ego_encoder = nn.Linear(self.state_dim['ego_vehicle'], 64) + self.fc = nn.Linear(256, 256) + self.fc_out = nn.Linear(256, self.action_parameter_size) + # torch.nn.init.normal_(self.fc1_1.weight.data,0,0.01) + # torch.nn.init.normal_(self.fc1_2.weight.data,0,0.01) + # torch.nn.init.normal_(self.fc_out.weight.data,0,0.01) + # torch.nn.init.normal_(self.fc_out.weight.data,0,0.01) + # torch.nn.init.xavier_normal_(self.fc1_1.weight.data) + # torch.nn.init.xavier_normal_(self.fc1_2.weight.data) + # torch.nn.init.xavier_normal_(self.fc_out.weight.data) + + def forward(self, state): + # state: (waypoints + 2 * companion_vehicle * 3 + one_state_dim = self.state_dim['waypoints'] + self.state_dim['companion_vehicle'] * 2 + self.state_dim['light'] + # print(state.shape, one_state_dim) + ego_info = state[:, 3 * one_state_dim:] + # print(ego_info.shape) + left_enc = self.left_encoder(state[:, :one_state_dim], ego_info) + center_enc = self.center_encoder(state[:, one_state_dim:2 * one_state_dim], ego_info) + right_enc = self.right_encoder(state[:, 2 * one_state_dim:3 * one_state_dim], ego_info) + ego_enc = self.ego_encoder(ego_info) + state_ = torch.cat((left_enc, center_enc, right_enc, ego_enc), dim=1) + hidden = F.relu(self.fc(state_)) + action = torch.tanh(self.fc_out(hidden)) + # steer,throttle_brake=torch.split(out,split_size_or_sections=[1,1],dim=1) + # steer=steer.clone() + # throttle_brake=throttle_brake.clone() + # steer*=self.action_bound['steer'] + # throttle=throttle_brake.clone() + # brake=throttle_brake.clone() + # for i in range(throttle.shape[0]): + # if throttle[i][0]<0: + # throttle[i][0]=0 + # if brake[i][0]>0: + # brake[i][0]=0 + # throttle*=self.action_bound['throttle'] + # brake*=self.action_bound['brake'] + + return action + + +class QValueNet_multi(torch.nn.Module): + def __init__(self, state_dim, action_param_dim, num_actions) -> None: + # parameter state_dim here is a dict + super().__init__() + self.state_dim = state_dim + self.action_param_dim = action_param_dim + self.num_actions = num_actions + self.left_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.center_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.right_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.ego_encoder = nn.Linear(self.state_dim['ego_vehicle'], 32) + self.action_encoder = nn.Linear(self.action_param_dim, 32) + self.fc = nn.Linear(256, 256) + self.fc_out = nn.Linear(256, self.num_actions) + + # torch.nn.init.normal_(self.fc1.weight.data,0,0.01) + # torch.nn.init.normal_(self.fc_out.weight.data,0,0.01) + # torch.nn.init.xavier_normal_(self.fc1.weight.data) + # torch.nn.init.xavier_normal_(self.fc_out.weight.data) + + def forward(self, state, action): + one_state_dim = self.state_dim['waypoints'] + self.state_dim['companion_vehicle'] * 2 + self.state_dim['light'] + ego_info = state[:, 3 * one_state_dim:] + left_enc = self.left_encoder(state[:, :one_state_dim], ego_info) + center_enc = self.center_encoder(state[:, one_state_dim:2 * one_state_dim], ego_info) + right_enc = self.right_encoder(state[:, 2 * one_state_dim:3 * one_state_dim], ego_info) + ego_enc = self.ego_encoder(ego_info) + action_enc = self.action_encoder(action) + state_ = torch.cat((left_enc, center_enc, right_enc, ego_enc, action_enc), dim=1) + hidden = F.relu(self.fc(state_)) + out = self.fc_out(hidden) + return out + + +class QValueNet_multi_td3(torch.nn.Module): + def __init__(self, state_dim, action_param_dim, num_actions) -> None: + # parameter state_dim here is a dict + super().__init__() + self.state_dim = state_dim + self.action_param_dim = action_param_dim + self.num_actions = num_actions + self.left_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.center_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.right_encoder = lane_wise_cross_attention_encoder(self.state_dim) + self.ego_encoder = nn.Linear(self.state_dim['ego_vehicle'], 32) + self.action_encoder = nn.Linear(self.action_param_dim, 32) + self.fc = nn.Linear(256, 256) + self.fc_out = nn.Linear(256, self.num_actions) + + self.left_encoder2 = lane_wise_cross_attention_encoder(self.state_dim) + self.center_encoder2 = lane_wise_cross_attention_encoder(self.state_dim) + self.right_encoder2 = lane_wise_cross_attention_encoder(self.state_dim) + self.ego_encoder2 = nn.Linear(self.state_dim['ego_vehicle'], 32) + self.action_encoder2 = nn.Linear(self.action_param_dim, 32) + self.fc2 = nn.Linear(256, 256) + self.fc_out2 = nn.Linear(256, self.num_actions) + + # torch.nn.init.normal_(self.fc1.weight.data,0,0.01) + # torch.nn.init.normal_(self.fc_out.weight.data,0,0.01) + # torch.nn.init.xavier_normal_(self.fc1.weight.data) + # torch.nn.init.xavier_normal_(self.fc_out.weight.data) + + def forward(self, state, action): + one_state_dim = self.state_dim['waypoints'] + self.state_dim['companion_vehicle'] * 2 + self.state_dim['light'] + ego_info = state[:, 3 * one_state_dim:] + + left_enc = self.left_encoder(state[:, :one_state_dim], ego_info) + center_enc = self.center_encoder(state[:, one_state_dim:2 * one_state_dim], ego_info) + right_enc = self.right_encoder(state[:, 2 * one_state_dim:3 * one_state_dim], ego_info) + ego_enc = self.ego_encoder(ego_info) + action_enc = self.action_encoder(action) + state_ = torch.cat((left_enc, center_enc, right_enc, ego_enc, action_enc), dim=1) + hidden = F.relu(self.fc(state_)) + out = self.fc_out(hidden) + + left_enc2 = self.left_encoder2(state[:, :one_state_dim], ego_info) + center_enc2 = self.center_encoder2(state[:, one_state_dim:2 * one_state_dim], ego_info) + right_enc2 = self.right_encoder2(state[:, 2 * one_state_dim:3 * one_state_dim], ego_info) + ego_enc2 = self.ego_encoder2(ego_info) + action_enc2 = self.action_encoder2(action) + state_2 = torch.cat((left_enc2, center_enc2, right_enc2, ego_enc2, action_enc2), dim=1) + hidden2 = F.relu(self.fc(state_2)) + out2 = self.fc_out(hidden2) + return out, out2 + + +class P_DQN: + def __init__(self, state_dim, action_dim, action_bound, gamma, tau, sigma, sigma_steer, sigma_acc, theta, epsilon, + buffer_size, batch_size, actor_lr, critic_lr, clip_grad, zero_index_gradients, inverting_gradients, + per_flag, device) -> None: + self.learn_time = 0 + self.replace_a = 0 + self.replace_c = 0 + self.s_dim = state_dim # state_dim here is a dict + self.s_dim['waypoints'] *= 3 # 2 is the feature dim of each waypoint + self.a_dim, self.a_bound = action_dim, action_bound + self.theta = theta + self.num_actions = 3 # left change, lane follow, right change + self.action_parameter_sizes = np.array([self.a_dim, self.a_dim, self.a_dim]) + self.action_parameter_size = int(self.action_parameter_sizes.sum()) + self.action_parameter_offsets = self.action_parameter_sizes.cumsum() + self.action_parameter_offsets = np.insert(self.action_parameter_offsets, 0, + 0) # [0, self.a_dim, self.a_dim*2, self.a_dim*3] + self.action_parameter_max_numpy = np.array([1, 1, 1, 1, 1, 1]) + self.action_parameter_min_numpy = np.array([-1, -1, -1, -1, -1, -1]) + self.action_parameter_range_numpy = np.array([2, 2, 2, 2, 2, 2]) + self.gamma, self.tau, self.sigma, self.epsilon = gamma, tau, sigma, epsilon # sigma:高斯噪声的标准差，均值直接设置为0 + self.steer_noise, self.tb_noise = sigma_steer, sigma_acc + self.batch_size, self.device = batch_size, device + self.actor_lr, self.critic_lr = actor_lr, critic_lr + self.clip_grad = clip_grad + self.indexd = zero_index_gradients + self.zero_index_gradients = zero_index_gradients + self.inverting_gradients = inverting_gradients + self.add_actor_noise = False + self.td3 = False + self.policy_freq = 2 + self.per_flag = per_flag + self.learn_time = 0 + # adjust different types of replay buffer + # self.replay_buffer = Split_ReplayBuffer(buffer_size) + if not self.per_flag: + self.replay_buffer = SplitReplayBuffer(buffer_size) + else: + self.replay_buffer = PriReplayBuffer(buffer_size) + # self.replay_buffer = offline_replay_buffer() + """self.memory=torch.tensor((buffer_size,self.s_dim*2+self.a_dim+1+1), + dtype=torch.float32).to(self.device)""" + self.pointer = 0 # serve as updating the memory data + self.train = True + self.actor = PolicyNet_multi(self.s_dim, self.action_parameter_size, self.a_bound).to(self.device) + self.actor_target = PolicyNet_multi(self.s_dim, self.action_parameter_size, self.a_bound).to(self.device) + self.actor_target.load_state_dict(self.actor.state_dict()) + if not self.td3: + self.critic = QValueNet_multi(self.s_dim, self.action_parameter_size, self.num_actions).to(self.device) + self.critic_target = QValueNet_multi(self.s_dim, self.action_parameter_size, self.num_actions).to( + self.device) + else: + self.critic = QValueNet_multi_td3(self.s_dim, self.action_parameter_size, self.num_actions).to(self.device) + self.critic_target = QValueNet_multi_td3(self.s_dim, self.action_parameter_size, self.num_actions).to( + self.device) + # self.actor = PolicyNet(self.s_dim, self.a_bound).to(self.device) + # self.actor_target = PolicyNet(self.s_dim, self.a_bound).to(self.device) + # self.actor_target.load_state_dict(self.actor.state_dict()) + # self.critic = QValueNet(self.s_dim, self.a_dim).to(self.device) + # self.critic_target = QValueNet(self.s_dim, self.a_dim).to(self.device) + self.critic_target.load_state_dict(self.critic.state_dict()) + + self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=actor_lr) + self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=critic_lr) + if self.per_flag: + self.loss = nn.MSELoss(reduction='none') + else: + self.loss = nn.MSELoss() + + # self.steer_noise = OrnsteinUhlenbeckActionNoise(self.sigma, self.theta) + # self.tb_noise = OrnsteinUhlenbeckActionNoise(self.sigma, self.theta) + + def take_action(self, state, lane_id=-2, action_mask=False): + # print('vehicle_info', state['vehicle_info']) + state_left_wps = torch.tensor(state['left_waypoints'], dtype=torch.float32).view(1, -1).to(self.device) + state_center_wps = torch.tensor(state['center_waypoints'], dtype=torch.float32).view(1, -1).to(self.device) + state_right_wps = torch.tensor(state['right_waypoints'], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_left_front = torch.tensor(state['vehicle_info'][0], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_front = torch.tensor(state['vehicle_info'][1], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_right_front = torch.tensor(state['vehicle_info'][2], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_left_rear = torch.tensor(state['vehicle_info'][3], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_rear = torch.tensor(state['vehicle_info'][4], dtype=torch.float32).view(1, -1).to(self.device) + state_veh_right_rear = torch.tensor(state['vehicle_info'][5], dtype=torch.float32).view(1, -1).to(self.device) + state_light = torch.tensor(state['light'], dtype=torch.float32).view(1, -1).to(self.device) + state_ev = torch.tensor(state['ego_vehicle'], dtype=torch.float32).view(1, -1).to(self.device) + state_ = torch.cat((state_left_wps, state_veh_left_front, state_veh_left_rear, state_light, + state_center_wps, state_veh_front, state_veh_rear, state_light, + state_right_wps, state_veh_right_front, state_veh_right_rear, state_light, state_ev), dim=1) + # print(state_.shape) + all_action_param = self.actor(state_) + if not self.td3: + q = self.critic(state_, all_action_param) + else: + q1, q2 = self.critic(state_, all_action_param) + q = torch.min(q1, q2) + q_a = torch.squeeze(q) + q_a = q_a.detach().cpu().numpy() + if action_mask: + if lane_id == -3: + q_a[2] = -1000000.0 + elif lane_id == -1: + q_a[0] = -1000000.0 + action = np.argmax(q_a) + action_param = all_action_param[ + :, self.action_parameter_offsets[action]:self.action_parameter_offsets[action + 1]] + + print(f'Network Output - Action: {action}, Steer: {action_param[0][0]}, Throttle_brake: {action_param[0][1]}') + print('q values: ', q_a) + if (action_param[0, 0].is_cuda): + action_param = np.array( + [action_param[:, 0].detach().cpu().numpy(), action_param[:, 1].detach().cpu().numpy()]).reshape((-1, 2)) + all_action_param = np.array( + [all_action_param[:, 0].detach().cpu().numpy(), all_action_param[:, 1].detach().cpu().numpy(), + all_action_param[:, 2].detach().cpu().numpy(), all_action_param[:, 3].detach().cpu().numpy(), + all_action_param[:, 4].detach().cpu().numpy(), all_action_param[:, 5].detach().cpu().numpy()]).reshape( + (-1, 6)) + else: + action_param = np.array([action_param[:, 0].detach().numpy(), action_param[:, 1].detach().numpy()]).reshape( + (-1, 2)) + all_action_param = np.array( + [all_action_param[:, 0].detach().numpy(), all_action_param[:, 1].detach().numpy(), + all_action_param[:, 2].detach().numpy(), all_action_param[:, 3].detach().numpy(), + all_action_param[:, 4].detach().numpy(), all_action_param[:, 5].detach().numpy()]).reshape((-1, 6)) + # if np.random.random() but Adam minimises, so reversed (could also double negate the grad) + index = grad > 0 + grad[index] *= (index.float() * (max_p - vals) / rnge)[index] + grad[~index] *= ((~index).float() * (vals - min_p) / rnge)[~index] + + return grad + + def learn(self): + self.learn_time += 1 + # if self.learn_time > 100000: + # self.train = False + self.replace_a += 1 + self.replace_c += 1 + if not self.per_flag: + b_s, b_a, b_a_param, b_r, b_ns, b_t, b_d = self.replay_buffer.sample(self.batch_size) + else: + b_idx, b_ISWeights, b_transition = self.replay_buffer.sample(self.batch_size) + b_s, b_a, b_a_param, b_r, b_ns, b_t, b_d, b_i = b_transition[0], b_transition[1], b_transition[2], \ + b_transition[3], b_transition[4], \ + b_transition[5], b_transition[6], b_transition[7] + self.ISWeights = torch.tensor(b_ISWeights, dtype=torch.float32).view((self.batch_size, -1)).to(self.device) + + # 此处得到的batch是否是pytorch.tensor? + batch_s = torch.tensor(b_s, dtype=torch.float32).view((self.batch_size, -1)).to(self.device) + batch_ns = torch.tensor(b_ns, dtype=torch.float32).view((self.batch_size, -1)).to(self.device) + batch_a = torch.tensor(b_a, dtype=torch.int64).view((self.batch_size, -1)).to(self.device) + batch_a_param = torch.tensor(b_a_param, dtype=torch.float32).view((self.batch_size, -1)).to(self.device) + batch_r = torch.tensor(b_r, dtype=torch.float32).view((self.batch_size, -1)).to(self.device).squeeze() + batch_d = torch.tensor(b_d, dtype=torch.float32).view((self.batch_size, -1)).to(self.device).squeeze() + batch_t = torch.tensor(b_t, dtype=torch.float32).view((self.batch_size, -1)).to(self.device).squeeze() + + with torch.no_grad(): + action_param_target = self.actor_target(batch_ns) + if self.add_actor_noise: + noise = (torch.rand_like(action_param_target) - 0.5) * 0.01 + noise = noise.clamp(-0.01, 0.01) + action_param_target = action_param_target + noise + if not self.td3: + q_target_values = self.critic_target(batch_ns, action_param_target) + else: + q_target_values1, q_target_values2 = self.critic_target(batch_ns, action_param_target) + q_target_values = torch.min(q_target_values1, q_target_values2) + q_prime = torch.max(q_target_values, 1, keepdim=True)[0].squeeze() + q_targets = batch_r + self.gamma * q_prime * (1 - batch_t) + if not self.td3: + q_values = self.critic(batch_s, batch_a_param) + q = q_values.gather(1, batch_a.view(-1, 1)).squeeze() + if not self.per_flag: + loss_q = self.loss(q, q_targets) + else: + loss = self.loss(q, q_targets) + abs_loss = torch.abs(q - q_targets) + abs_loss = np.array(abs_loss.detach().cpu().numpy()) + loss_q = torch.mean(loss * self.ISWeights) + self.replay_buffer.batch_update(b_idx, abs_loss) + else: + q_values1, q_values2 = self.critic(batch_s, batch_a_param) + q_values = torch.min(q_values1, q_values2) + q = q_values.gather(1, batch_a.view(-1, 1)).squeeze() + loss_q = self.loss(q, q_values1) + self.loss(q, q_values2) + + print("Loss_Q:", loss_q) + + self.critic_optimizer.zero_grad() + loss_q.backward() + if self.clip_grad > 0: + torch.nn.utils.clip_grad_norm_(self.critic.parameters(), self.clip_grad) + self.critic_optimizer.step() + + if self.learn_time % self.policy_freq == 0: + with torch.no_grad(): + action_param = self.actor(batch_s) + action_param.requires_grad = True + if not self.td3: + Q = self.critic(batch_s, action_param) + Q_val = Q + else: + Q1, Q2 = self.critic(batch_s, action_param) + Q_val = torch.min(Q1, Q2) + if self.indexd: + Q_indexed = Q_val.gather(1, batch_a.view(-1, 1)) + Q_loss = torch.mean(Q_indexed) + else: + Q_loss = torch.mean(torch.sum(Q_val, 1)) + + self.critic.zero_grad() + Q_loss.backward() + from copy import deepcopy + # print('check batch_s whether has grad: ', batch_s.grad_fn) + delta_a = deepcopy(action_param.grad.data) + + action_param = self.actor(Variable(batch_s)) + delta_a[:] = self._invert_gradients(delta_a, action_param, grad_type="action_parameters", inplace=True) + if self.zero_index_gradients: + delta_a[:] = self._zero_index_gradients(delta_a, batch_action_indices=batch_a, inplace=True) + + out = -torch.mul(delta_a, action_param) + self.actor.zero_grad() + out.backward(torch.ones(out.shape).to(self.device)) + if self.clip_grad > 0: + torch.nn.utils.clip_grad_norm_(self.actor.parameters(), self.clip_grad) + self.actor_optimizer.step() + self.soft_update(self.actor, self.actor_target) + self.soft_update(self.critic, self.critic_target) + + def _print_grad(self, model): + '''Print the grad of each layer''' + for name, parms in model.named_parameters(): + print('-->name:', name, '-->grad_requirs:', parms.requires_grad, ' -->grad_value:', parms.grad) + + def set_sigma(self, sigma_steer, sigma_acc): + # self.sigma = sigma + self.steer_noise = sigma_steer + self.tb_noise = sigma_acc + + def reset_noise(self): + pass + # self.steer_noise.reset() + # self.tb_noise.reset() + + def soft_update(self, net, target_net): + for param_target, param in zip(target_net.parameters(), net.parameters()): + param_target.data.copy_(param_target.data * (1.0 - self.tau) + param.data * self.tau) + + def hard_update(self, net, target_net): + net.load_state_dict(target_net.state_dict()) + + def store_transition(self, state, action, action_param, reward, next_state, truncated, done, info): + # how to store the episodic data to buffer + def _compress(state): + # print('state: ', state) + state_left_wps = np.array(state['left_waypoints'], dtype=np.float32).reshape((1, -1)) + state_center_wps = np.array(state['center_waypoints'], dtype=np.float32).reshape((1, -1)) + state_right_wps = np.array(state['right_waypoints'], dtype=np.float32).reshape((1, -1)) + state_veh_left_front = np.array(state['vehicle_info'][0], dtype=np.float32).reshape((1, -1)) + state_veh_front = np.array(state['vehicle_info'][1], dtype=np.float32).reshape((1, -1)) + state_veh_right_front = np.array(state['vehicle_info'][2], dtype=np.float32).reshape((1, -1)) + state_veh_left_rear = np.array(state['vehicle_info'][3], dtype=np.float32).reshape((1, -1)) + state_veh_rear = np.array(state['vehicle_info'][4], dtype=np.float32).reshape((1, -1)) + state_veh_right_rear = np.array(state['vehicle_info'][5], dtype=np.float32).reshape((1, -1)) + state_ev = np.array(state['ego_vehicle'], dtype=np.float32).reshape((1, -1)) + state_light = np.array(state['light'], dtype=np.float32).reshape((1, -1)) + state_ = np.concatenate((state_left_wps, state_veh_left_front, state_veh_left_rear, state_light, + state_center_wps, state_veh_front, state_veh_rear, state_light, + state_right_wps, state_veh_right_front, state_veh_right_rear, state_light, + state_ev), axis=1) + return state_ + + state = _compress(state) + next_state = _compress(next_state) + + if not truncated: + lane_center = info["offlane"] + reward_ttc = info["TTC"] + reward_eff = info["velocity"] + reward_com = info["Comfort"] + reward_eff = info["velocity"] + reward_yaw = info["yaw_diff"] + # if reward_ttc < -0.1 or reward_eff < 3: + # self.change_buffer.append((state, action, action_param, reward, next_state, truncated, done)) + # if truncated: + # self.change_buffer.append((state, action, action_param, reward, next_state, truncated, done)) + if not self.per_flag: + if action == 0 or action == 2: + self.replay_buffer.add((state, action, action_param, reward, next_state, truncated, done), False) + self.replay_buffer.add((state, action, action_param, reward, next_state, truncated, done), True) + else: + self.replay_buffer.add((state, action, action_param, reward, next_state, truncated, done, info)) + # print("their shapes", state, action, next_state, reward_list, truncated, done) + # state: [1, 28], action: [1, 2], next_state: [1, 28], reward_list = [1, 6], truncated = [1, 1], done = [1, 1] + # all: [1, 66] + + return + + def save_net(self, file='./out/ddpg_final.pth'): + state = { + 'actor': self.actor.state_dict(), + 'actor_target': self.actor_target.state_dict(), + 'critic': self.critic.state_dict(), + 'critic_target': self.critic_target.state_dict(), + 'actor_optimizer': self.actor_optimizer.state_dict(), + 'critic_optimizer': self.critic_optimizer.state_dict() + } + torch.save(state, file) + + def load_net(self, state): + if state is not None: + if 'critic' in state: + self.critic.load_state_dict(state['critic']) + if 'critic_target' in state: + self.critic_target.load_state_dict(state['critic_target']) + if 'actor' in state: + self.actor.load_state_dict(state['actor']) + if 'actor_target' in state: + self.actor_target.load_state_dict(state['actor_target']) + if 'actor_optimizer' in state: + self.actor_optimizer.load_state_dict(state['actor_optimizer']) + if 'critic_optimizer' in state: + self.critic_optimizer.load_state_dict(state['critic_optimizer']) diff --git a/src/distributed_hierarchical_attentive/algs/util/replay_buffer.py b/src/distributed_hierarchical_attentive/algs/util/replay_buffer.py new file mode 100644 index 0000000000..234fc32d53 --- /dev/null +++ b/src/distributed_hierarchical_attentive/algs/util/replay_buffer.py @@ -0,0 +1,275 @@ +import logging +import random, collections +import numpy as np + + +class ReplayBuffer: + """经验回放池""" + + def __init__(self, capacity) -> None: + self.buffer = collections.deque(maxlen=capacity) # 队列，先进先出 + + def add(self, transition): + self.buffer.append(transition) + + def sample(self, batch_size): # 从buffer中采样数据,数量为batch_size + transition = random.sample(self.buffer, batch_size) + state, action, reward, next_state, truncated, done, info = zip(*transition) + return state, action, reward, next_state, truncated, done, info + + def size(self): + return len(self.buffer) + + +class SplitReplayBuffer: + + def __init__(self, capacity) -> None: + self.buffer = collections.deque(maxlen=capacity) # 队列，先进先出 + self.change_buffer = collections.deque(maxlen=capacity // 10) + + def add(self, transition, buffer=True): + """Transiton: the rl transition need to sava + buffer: True, add transition to self.buffer; False, add transition to self.change_buffer""" + if buffer: + self.buffer.append(transition) + else: + self.change_buffer.append(transition) + + def sample(self, batch_size): # 从buffer中采样数据,数量为batch_size + pri_size = min(batch_size // 2, len(self.change_buffer)) + normal_size = batch_size - pri_size + transition = random.sample(self.buffer, normal_size) + state, action, action_param, reward, next_state, truncated, done = zip(*transition) + pri_transition = random.sample(self.change_buffer, pri_size) + pri_state, pri_action, pri_action_param, pri_reward, pri_next_state, pri_truncated, pri_done = zip( + *pri_transition) + state = np.concatenate((state, pri_state), axis=0) + action = np.concatenate((action, pri_action), axis=0) + action_param = np.concatenate((action_param, pri_action_param), axis=0) + reward = np.concatenate((reward, pri_reward), axis=0) + next_state = np.concatenate((next_state, pri_next_state), axis=0) + truncated = np.concatenate((truncated, pri_truncated), axis=0) + done = np.concatenate((done, pri_done), axis=0) + return state, action, action_param, reward, next_state, truncated, done + + def size(self): + return len(self.buffer) + + +class OfflineReplayBuffer: + """ + manually adjust the sampling of replay buffer and the replay buffer remains unchanged (1,000,000 buffers) + """ + + def __init__(self): + file_path = "../out/all_replay_buffer.npy" + # state: [1, 28], action: [1, 2], next_state: [1, 28], reward_list = [1, 6], truncated = [1, 1], done = [1, 1] + # reward_list = np.array([[reward, reward_ttc, reward_com, reward_eff, reward_lan, reward_yaw]]) + self.replay_buffer = np.load(file_path, allow_pickle=True) + self.buffer_num = self.replay_buffer.shape[0] + # print(replay_buffer.shape) + # split five buffer of different states: dangerous, large off-center, low-efficiency, on-curve, normal + self.dangerous_buffer = collections.deque(maxlen=250000) + self.off_center_buffer = collections.deque(maxlen=250000) + self.low_efficiency_buffer = collections.deque(maxlen=250000) + self.on_curve_buffer = collections.deque(maxlen=250000) + self.normal_buffer = collections.deque(maxlen=1000000) + + self.ttc_thr = -0.00001 + self.lane_thr = 0.5 + self.eff_thr = 5 + self.curve_thr = 0.1 + self.split_replay_buffer() + + def size(self): + return self.buffer_num + + def split_replay_buffer(self): + for i in range(self.buffer_num): + current_buffer = self.replay_buffer[i] + fTTC = current_buffer[59] + fLane = current_buffer[62] + feff = current_buffer[61] + fcurve = abs(current_buffer[1] - current_buffer[19]) + print("fttc: ", fTTC, "flane: ", fLane, "feff: ", feff, "fcurve: ", fcurve) + if fTTC < self.ttc_thr: + # print(fTTC) + self.dangerous_buffer.append(current_buffer) + elif fLane > self.lane_thr: + self.off_center_buffer.append(current_buffer) + elif feff < self.eff_thr: + self.low_efficiency_buffer.append(current_buffer) + elif fcurve > self.curve_thr: + self.on_curve_buffer.append(current_buffer) + else: + self.normal_buffer.append(current_buffer) + print("dangerous_buffer: ", len(self.dangerous_buffer), "off_center_buffer: ", len(self.off_center_buffer), + "low_efficiency_buffer: ", len(self.low_efficiency_buffer), "on_curve_buffer: ", + len(self.on_curve_buffer), "normal_buffer: ", len(self.normal_buffer)) + + def sample(self, batch_size): + specific_size = batch_size / 6 + normal_size = batch_size - 4 * specific_size + dangerous_transition = random.sample(self.dangerous_buffer, specific_size) + dangerous_transitions = zip(*dangerous_transition) + off_center_transition = random.sample(self.off_center_buffer, specific_size) + off_center_transitions = zip(*off_center_transition) + low_efficiency_transition = random.sample(self.low_efficiency_buffer, specific_size) + low_efficiency_transitions = zip(*low_efficiency_transition) + on_curve_transition = random.sample(self.on_curve_buffer, specific_size) + on_curve_transitions = zip(*on_curve_transition) + normal_transition = random.sample(self.normal_buffer, normal_size) + normal_transitions = zip(*normal_transition) + all_transitions = np.concatenate((dangerous_transitions, off_center_transitions, on_curve_transitions, + low_efficiency_transitions, normal_transitions), axis=0) + state, action, next_state, reward, truncated, done = all_transitions[:, :28], all_transitions[:, 28:30], + all_transitions[:, 30:56], all_transitions[:, 56:57], all_transitions[:, -2:-1], all_transitions[:, -1:] + return state, action, reward, next_state, truncated, done + + +class SumTree(object): + """ + This SumTree code is a modified version and the original code is from: + https://github.com/jaara/AI-blog/blob/master/SumTree.py + + Story data with its priority in the tree. + """ + data_pointer = 0 + + def __init__(self, capacity): + self.capacity = capacity # for all priority values + self.tree = np.zeros(2 * capacity - 1) + # [--------------Parent nodes-------------][-------leaves to recode priority-------] + # size: capacity - 1 size: capacity + # self.data = np.zeros(capacity, dtype=object) # for all transitions + self.data = collections.deque(maxlen=capacity) + # [--------------data frame-------------] + # size: capacity + + def add(self, p, transition): + tree_idx = self.data_pointer + self.capacity - 1 + # self.data[self.data_pointer] = data # update data_frame + if self.size < self.capacity: + self.data.append(transition) + else: + self.data[self.data_pointer] = transition + self.update(tree_idx, p) # update tree_frame + + self.data_pointer += 1 + if self.data_pointer >= self.capacity: # replace when exceed the capacity + self.data_pointer = 0 + + def update(self, tree_idx, p): + change = p - self.tree[tree_idx] + self.tree[tree_idx] = p + # then propagate the change through tree + while tree_idx != 0: # this method is faster than the recursive loop in the reference code + tree_idx = (tree_idx - 1) // 2 + self.tree[tree_idx] += change + + def get_leaf(self, v): + """ + Tree structure and array storage: + + Tree index: + 0 -> storing priority sum + / \ + 1 2 + / \ / \ + 3 4 5 6 -> storing priority for transitions + + Array type for storing: + [0,1,2,3,4,5,6] + """ + parent_idx = 0 + while True: # the while loop is faster than the method in the reference code + cl_idx = 2 * parent_idx + 1 # this leaf's left and right kids + cr_idx = cl_idx + 1 + if cl_idx >= self.capacity + self.size - 1: + # if cl_idx >= len(self.tree): # reach bottom, end search + leaf_idx = parent_idx + break + else: # downward search, always search for a higher priority node + if v <= self.tree[cl_idx]: + parent_idx = cl_idx + else: + v -= self.tree[cl_idx] + parent_idx = cr_idx + + if leaf_idx < self.capacity - 1: + # not leaf, banch node + leaf_idx = self.capacity - 1 + self.size - 1 + data_idx = leaf_idx - self.capacity + 1 + return leaf_idx, self.tree[leaf_idx], self.data[data_idx] + + @property + def total_p(self): + return self.tree[0] # the root + + @property + def size(self): + return len(self.data) + + +class PriReplayBuffer(object): # stored as ( s, a, r, s_, i ) in SumTree + """ + Prioritized experience replay + This Memory class is modified based on the original code from: + https://github.com/jaara/AI-blog/blob/master/Seaquest-DDQN-PER.py + Detailed information: + https://yulizi123.github.io/tutorials/machine-learning/reinforcement-learning/4-6-prioritized-replay/ + """ + epsilon = 0.01 # small amount to avoid zero priority + alpha = 0.6 # [0~1] convert the importance of TD error to priority. If alpha = 0, there is no Importance Sampling. + beta = 0.4 # importance-sampling, from initial value increasing to 1 + beta_increment_per_sampling = 0.001 + abs_err_upper = 1. # clipped abs error + + def __init__(self, capacity): + self.tree = SumTree(capacity) + + def add(self, transition): + max_p = np.max(self.tree.tree[-self.tree.capacity:]) + if max_p == 0: + max_p = self.abs_err_upper + self.tree.add(max_p, transition) # set the max p for new p + + def sample(self, n): + # assert self.tree.size==self.tree.capacity + b_idx, ISWeights = np.empty((n,), dtype=np.int32), np.empty((n, 1)) + b_state, b_action, b_reward, b_next_state, b_truncated, b_done, b_info = [], [], [], [], [], [], [] + pri_seg = self.tree.total_p / n # priority segment + self.beta = np.min([1., self.beta + self.beta_increment_per_sampling]) # max = 1 + + min_prob = np.min( + self.tree.tree[self.tree.capacity - 1:self.tree.capacity - 1 + self.size()]) / self.tree.total_p + # min_prob = np.min(self.tree.tree[-self.tree.capacity:]) / self.tree.total_p # for later calculate ISweight + for i in range(n): + a, b = pri_seg * i, pri_seg * (i + 1) + v = np.random.uniform(a, b) # sample from [a, b) + idx, p, data = self.tree.get_leaf(v) + prob = p / self.tree.total_p + ISWeights[i, 0] = np.power(prob / min_prob, -self.beta) + b_idx[i] = idx + b_state.append(data[0]) + b_action.append(data[1]) + b_reward.append(data[2]) + b_next_state.append(data[3]) + b_truncated.append(data[4]) + b_done.append(data[5]) + b_info.append(data[6]) + + # print(self.tree.tree) + # print(b_idx) + return b_idx, ISWeights, (np.array(b_state), np.array(b_action), np.array(b_reward), np.array(b_next_state), + np.array(b_truncated), np.array(b_done), np.array(b_info)) + + def batch_update(self, tree_idx, abs_errors): + abs_errors += self.epsilon # convert to abs and avoid 0 + clipped_errors = np.minimum(abs_errors, self.abs_err_upper) + ps = np.power(clipped_errors, self.alpha) + for ti, p in zip(tree_idx, ps): + self.tree.update(ti, p) + + def size(self): + return self.tree.size diff --git a/src/distributed_hierarchical_attentive/figures/Highway_route.png b/src/distributed_hierarchical_attentive/figures/Highway_route.png new file mode 100644 index 0000000000..2da990fec2 Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/Highway_route.png differ diff --git a/src/distributed_hierarchical_attentive/figures/Lane_change.gif b/src/distributed_hierarchical_attentive/figures/Lane_change.gif new file mode 100644 index 0000000000..0a52e1d16f Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/Lane_change.gif differ diff --git a/src/distributed_hierarchical_attentive/figures/Urban_route_1.png b/src/distributed_hierarchical_attentive/figures/Urban_route_1.png new file mode 100644 index 0000000000..57702bc819 Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/Urban_route_1.png differ diff --git a/src/distributed_hierarchical_attentive/figures/Urban_route_2.png b/src/distributed_hierarchical_attentive/figures/Urban_route_2.png new file mode 100644 index 0000000000..019e7dc735 Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/Urban_route_2.png differ diff --git a/src/distributed_hierarchical_attentive/figures/Urban_route_3.png b/src/distributed_hierarchical_attentive/figures/Urban_route_3.png new file mode 100644 index 0000000000..f707e01f6a Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/Urban_route_3.png differ diff --git a/src/distributed_hierarchical_attentive/figures/curve1.png b/src/distributed_hierarchical_attentive/figures/curve1.png new file mode 100644 index 0000000000..a74c4a4895 Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/curve1.png differ diff --git a/src/distributed_hierarchical_attentive/figures/curve2.png b/src/distributed_hierarchical_attentive/figures/curve2.png new file mode 100644 index 0000000000..98fcffde5a Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/curve2.png differ diff --git a/src/distributed_hierarchical_attentive/figures/framework.png b/src/distributed_hierarchical_attentive/figures/framework.png new file mode 100644 index 0000000000..4446b11894 Binary files /dev/null and b/src/distributed_hierarchical_attentive/figures/framework.png differ diff --git a/src/distributed_hierarchical_attentive/gym_carla/__init__.py b/src/distributed_hierarchical_attentive/gym_carla/__init__.py new file mode 100644 index 0000000000..2ad5635f11 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/__init__.py @@ -0,0 +1,6 @@ +from gym.envs.registration import register + +register( + id='CarlaEnv-v0', + entry_point='gym_carla.env.carla_env:CarlaEnv' +) \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/__init__.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/__init__.py new file mode 100644 index 0000000000..bc29ece582 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/__init__.py @@ -0,0 +1 @@ +#from gym_carla.env import CarlaEnv \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_agent.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_agent.py new file mode 100644 index 0000000000..cb548b6355 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_agent.py @@ -0,0 +1,330 @@ +import carla +from shapely.geometry import Polygon +from gym_carla.multi_lane.agent.pid_controller import VehiclePIDController +from gym_carla.multi_lane.util.misc import get_speed, is_within_distance, get_trafficlight_trigger_location, \ + compute_distance + + +# +class BasicAgent(object): + """ + BasicAgent implements an agent that navigates the scene. + This agent respects traffic lights and other vehicles, but ignores stop signs. + It has several functions available to specify the route that the agent must follow, + as well as to change its parameters in case a different driving mode is desired. + """ + + def __init__(self, vehicle, opt_dict={}): + """ + Initialization the agent paramters, the local and the global planner. + + :param vehicle: actor to apply to agent logic onto + :param target_speed: speed (in Km/h) at which the vehicle will move + :param opt_dict: dictionary in case some of its parameters want to be changed. + This also applies to parameters related to the LocalPlanner. + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + self._last_traffic_light = None + + # Base parameters + self._ignore_traffic_lights = True + self._ignore_stop_signs = True + self._ignore_vehicles = False + self._target_speed = 20 + self._base_tlight_threshold = 5.0 # meters + self._base_vehicle_threshold = 5.0 # meters + + # Controller parameters + self._max_throt = 1.0 + self._max_brake = 0.3 + self._max_steer = 0.8 + self._offset = 0 + self._follow_speed_limits = False + + # Change parameters according to the dictionary + if opt_dict: + if 'dt' in opt_dict: + self._dt = opt_dict['dt'] + if 'target_speed' in opt_dict: + self._target_speed = opt_dict['target_speed'] + if 'lateral_control_dict' in opt_dict: + self._args_lateral_dict = opt_dict['lateral_control_dict'] + else: + self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} + if 'longitudinal_control_dict' in opt_dict: + self._args_longitudinal_dict = opt_dict['longitudinal_control_dict'] + else: + self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} + if 'max_throttle' in opt_dict: + self._max_throt = opt_dict['max_throttle'] + if 'max_brake' in opt_dict: + self._max_brake = opt_dict['max_brake'] + if 'max_steering' in opt_dict: + self._max_steer = opt_dict['max_steering'] + if 'offset' in opt_dict: + self._offset = opt_dict['offset'] + if 'ignore_traffic_lights' in opt_dict: + self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] + if 'ignore_stop_signs' in opt_dict: + self._ignore_stop_signs = opt_dict['ignore_stop_signs'] + if 'ignore_vehicles' in opt_dict: + self._ignore_vehicles = opt_dict['ignore_vehicles'] + if 'base_tlight_threshold' in opt_dict: + self._base_tlight_threshold = opt_dict['base_tlight_threshold'] + if 'base_vehicle_threshold' in opt_dict: + self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] + if 'follow_speed_limits' in opt_dict: + self._follow_speed_limits = opt_dict['follow_speed_limits'] + + # Initialize the controller + self._vehicle_controller = VehiclePIDController(self._vehicle, args_lateral=self._args_lateral_dict, + args_longitudinal=self._args_longitudinal_dict, + offset=self._offset, max_throttle=self._max_throt, + max_brake=self._max_brake, max_steering=self._max_steer) + + def run_step(self, road_info): + """Execute one step of navigation.""" + # hazard_detected = False + + # # Retrieve all relevant actors + # actor_list = self._world.get_actors() + # vehicle_list = actor_list.filter("*vehicle*") + # lights_list = actor_list.filter("*traffic_light*") + + # vehicle_speed = get_speed(self._vehicle) / 3.6 + + # # Check for possible vehicle obstacles + # max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed + # affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) + # if affected_by_vehicle: + # hazard_detected = True + + # # Check if the vehicle is affected by a red traffic light + # max_tlight_distance = self._base_tlight_threshold + vehicle_speed + # affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) + # if affected_by_tlight: + # hazard_detected = True + + if self.follow_speed_limits: + self._target_speed = self._vehicle.get_speed_limit() + + if road_info['waypoints'] is None: + # Stop if no target waypoint + control = carla.VehicleControl() + control.steer = 0 + control.throttle = 0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + elif road_info['vehicle_front']: + control = self.emergency_stop() + else: + control = self._vehicle_controller.run_step(self._target_speed, road_info['waypoints'][0]) + + return control + + def done(self): + """Check whether the agent has reached its destination.""" + return len(self._waypoints_queue) == 0 + + def ignore_traffic_lights(self, active=True): + """(De)activates the checks for traffic lights""" + self._ignore_traffic_lights = active + + def ignore_stop_signs(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_stop_signs = active + + def ignore_vehicles(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_vehicles = active + + def emergency_stop(self): + """ + Overwrites the throttle a brake values of a control to perform an emergency stop. + The steering is kept the same to avoid going out of the lane when stopping during turns + + :param speed (carl.VehicleControl): control to be modified + """ + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control + + def set_target_speed(self, speed): + """ + Changes the target speed of the agent + :param speed (float): target speed in Km/h + """ + if self._follow_speed_limits: + print("WARNING: The max speed is currently set to follow the speed limits. " + "Use 'follow_speed_limits' to deactivate this") + self._target_speed = speed + + def follow_speed_limits(self, value=True): + """ + If active, the agent will dynamically change the target speed according to the speed limits + + :param value (bool): whether or not to activate this behavior + """ + self._follow_speed_limits = value + + def _affected_by_traffic_light(self, lights_list=None, max_distance=None): + """ + Method to check if there is a red light affecting the vehicle. + + :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. + If None, all traffic lights in the scene are used + :param max_distance (float): max distance for traffic lights to be considered relevant. + If None, the base threshold value is used + """ + if self._ignore_traffic_lights: + return (False, None) + + if not lights_list: + lights_list = self._world.get_actors().filter("*traffic_light*") + + if not max_distance: + max_distance = self._base_tlight_threshold + + if self._last_traffic_light: + if self._last_traffic_light.state != carla.TrafficLightState.Red: + self._last_traffic_light = None + else: + return (True, self._last_traffic_light) + + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + for traffic_light in lights_list: + object_location = get_trafficlight_trigger_location(traffic_light) + object_waypoint = self._map.get_waypoint(object_location) + + if object_waypoint.road_id != ego_vehicle_waypoint.road_id: + continue + + ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() + wp_dir = object_waypoint.transform.get_forward_vector() + dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z + + if dot_ve_wp < 0: + continue + + if traffic_light.state != carla.TrafficLightState.Red: + continue + + if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): + self._last_traffic_light = traffic_light + return (True, traffic_light) + + return (False, None) + + def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, + lane_offset=0): + """ + Method to check if there is a vehicle in front of the agent blocking its path. + + :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. + If None, all vehicle in the scene are used + :param max_distance: max freespace to check for obstacles. + If None, the base threshold value is used + """ + if self._ignore_vehicles: + return (False, None, -1) + + if not vehicle_list: + vehicle_list = self._world.get_actors().filter("*vehicle*") + + if not max_distance: + max_distance = self._base_vehicle_threshold + + ego_transform = self._vehicle.get_transform() + ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) + + # Get the right offset + if ego_wpt.lane_id < 0 and lane_offset != 0: + lane_offset *= -1 + + # Get the transform of the front of the ego + ego_forward_vector = ego_transform.get_forward_vector() + ego_extent = self._vehicle.bounding_box.extent.x + ego_front_transform = ego_transform + ego_front_transform.location += carla.Location( + x=ego_extent * ego_forward_vector.x, + y=ego_extent * ego_forward_vector.y, + ) + + for target_vehicle in vehicle_list: + target_transform = target_vehicle.get_transform() + target_wpt = self._map.get_waypoint(target_transform.location, lane_type=carla.LaneType.Any) + + # Simplified version for outside junctions + if not ego_wpt.is_junction or not target_wpt.is_junction: + + if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: + next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] + if not next_wpt: + continue + if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: + continue + + target_forward_vector = target_transform.get_forward_vector() + target_extent = target_vehicle.bounding_box.extent.x + target_rear_transform = target_transform + target_rear_transform.location -= carla.Location( + x=target_extent * target_forward_vector.x, + y=target_extent * target_forward_vector.y, + ) + + if is_within_distance(target_rear_transform, ego_front_transform, max_distance, + [low_angle_th, up_angle_th]): + return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) + + # Waypoints aren't reliable, check the proximity of the vehicle to the route + else: + route_bb = [] + ego_location = ego_transform.location + extent_y = self._vehicle.bounding_box.extent.y + r_vec = ego_transform.get_right_vector() + p1 = ego_location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) + p2 = ego_location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) + route_bb.append([p1.x, p1.y, p1.z]) + route_bb.append([p2.x, p2.y, p2.z]) + + for wp, _ in self._local_planner.get_plan(): + if ego_location.distance(wp.transform.location) > max_distance: + break + + r_vec = wp.transform.get_right_vector() + p1 = wp.transform.location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) + p2 = wp.transform.location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) + route_bb.append([p1.x, p1.y, p1.z]) + route_bb.append([p2.x, p2.y, p2.z]) + + if len(route_bb) < 3: + # 2 points don't create a polygon, nothing to check + return (False, None, -1) + ego_polygon = Polygon(route_bb) + + # Compare the two polygons + for target_vehicle in vehicle_list: + target_extent = target_vehicle.bounding_box.extent.x + if target_vehicle.id == self._vehicle.id: + continue + if ego_location.distance(target_vehicle.get_location()) > max_distance: + continue + + target_bb = target_vehicle.bounding_box + target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform()) + target_list = [[v.x, v.y, v.z] for v in target_vertices] + target_polygon = Polygon(target_list) + + if ego_polygon.intersects(target_polygon): + return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location)) + + return (False, None, -1) + + return (False, None, -1) diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_lanechanging_agent.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_lanechanging_agent.py new file mode 100644 index 0000000000..eb16d0fc4f --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/basic_lanechanging_agent.py @@ -0,0 +1,427 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# +""" +This module implements an agent that roams around a track following random +waypoints and avoiding other vehicles. The agent also responds to traffic lights. +It can also make use of the global route planner to follow a specifed route +""" + +import carla +import random +import numpy as np +from enum import Enum +from collections import deque +from shapely.geometry import Polygon +from gym_carla.multi_lane.util.wrapper import Action, ControlInfo +from gym_carla.multi_lane.agent.pid_controller import VehiclePIDController +from gym_carla.multi_lane.util.misc import get_speed, draw_waypoints, is_within_distance, \ + get_trafficlight_trigger_location, \ + compute_distance, get_lane_center + + +class Basic_Lanechanging_Agent(object): + """ + BasicAgent implements an agent that navigates the scene. + This agent respects traffic lights and other vehicles, but ignores stop signs. + It has several functions available to specify the route that the agent must follow, + as well as to change its parameters in case a different driving mode is desired. + """ + + def __init__(self, vehicle, dt=1.0 / 20, opt_dict={}): + """ + Initialization the agent paramters, the local and the global planner. + + :param vehicle: actor to apply to agent logic onto + :param target_speed: speed (in Km/h) at which the vehicle will move + :param opt_dict: dictionary in case some of its parameters want to be changed. + This also applies to parameters related to the LocalPlanner. + """ + self._vehicle = vehicle + self._vehicle_location = self._vehicle.get_location() + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + self._last_traffic_light = None + + # Base parameters + self._ignore_traffic_lights = False + self._ignore_stop_signs = False + self._ignore_vehicle = False + self._ignore_change_gap = False + self.lanechanging_fps = 50 + + # PID controller parameter + self._dt = dt + self._target_speed = 20.0 # Km/h + self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} + self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} + self._max_throt = 0.75 + self._max_brake = 0.3 + self._max_steer = 0.8 + self._offset = 0 + self._base_min_distance = 3.0 + self._follow_speed_limits = False + + self._sampling_resolution = 2.0 + self._base_tlight_threshold = 5.0 # meters + self._base_vehicle_threshold = 10.0 # meters + self.lane_change_mode = False + self.last_lane = None + self.autopilot_step = 0 + self.random_lane_change = True + + # set by carla_env.py + self.left_wps = [] + self.center_wps = [] + self.right_wps = [] + self.left_rear_wps = [] + self.center_rear_wps = [] + self.right_rear_wps = [] + + self.distance_to_left_front = None + self.distance_to_center_front = None + self.distance_to_right_front = None + self.distance_to_left_rear = None + self.distance_to_center_rear = None + self.distance_to_right_rear = None + + self.left_next_wayppoint = None + self.center_next_waypoint = None + self.right_next_waypoint = None + + self.enable_left_change = True + self.enable_right_change = True + + # Change parameters according to the dictionary + if 'ignore_traffic_lights' in opt_dict: + self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] + if 'ignore_stop_signs' in opt_dict: + self._ignore_stop_signs = opt_dict['ignore_stop_signs'] + if 'sampling_resolution' in opt_dict: + self._sampling_resolution = opt_dict['sampling_resolution'] + if 'base_tlight_threshold' in opt_dict: + self._base_tlight_threshold = opt_dict['base_tlight_threshold'] + if 'base_vehicle_threshold' in opt_dict: + self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] + if 'max_steering' in opt_dict: + self._max_steer = opt_dict['max_steering'] + if 'max_throttle' in opt_dict: + self._max_throt = opt_dict['max_throttle'] + if 'max_brake' in opt_dict: + self._max_brake = opt_dict['max_brake'] + if 'buffer_size' in opt_dict: + self._buffer_size = opt_dict['buffer_size'] + if 'ignore_front_vehicle' in opt_dict: + self._ignore_vehicle = opt_dict['ignore_front_vehicle'] + if 'ignore_change_gap' in opt_dict: + self._ignore_change_gap = opt_dict['ignore_change_gap'] + if 'lanechanging_fps' in opt_dict: + self.lanechanging_fps = opt_dict['lanechanging_fps'] + if 'target_speed' in opt_dict: + self._target_speed = opt_dict['target_speed'] + if 'random_lane_change' in opt_dict: + self.random_lane_change = opt_dict['random_lane_change'] + + print('ignore_front_vehicle, ignore_change_gap: ', self._ignore_vehicle, self._ignore_change_gap) + + self.left_random_change = [] + self.center_random_change = [] + self.right_random_change = [] + self.init_random_change() + + self._vehicle_controller = VehiclePIDController(self._vehicle, + args_lateral=self._args_lateral_dict, + args_longitudinal=self._args_longitudinal_dict, + offset=self._offset, + max_throttle=self._max_throt, + max_brake=self._max_brake, + max_steering=self._max_steer) + + def init_random_change(self): + for i in range(self.lanechanging_fps): + self.left_random_change.append(Action.LANE_FOLLOW) + self.center_random_change.append(Action.LANE_FOLLOW) + # center_random_change.append(0) + self.right_random_change.append(Action.LANE_FOLLOW) + self.left_random_change.append(Action.LANE_CHANGE_RIGHT) + self.center_random_change.append(Action.LANE_CHANGE_RIGHT) + self.center_random_change.append(Action.LANE_CHANGE_LEFT) + self.right_random_change.append(Action.LANE_CHANGE_LEFT) + + def add_emergency_stop(self, control): + """ + Overwrites the throttle a brake values of a control to perform an emergency stop. + The steering is kept the same to avoid going out of the lane when stopping during turns + + :param speed (carl.VehicleControl): control to be modified + """ + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control + + def set_info(self, info_dict): + """ + :param left_wps: waypoints in left-front lane + :param center_wps: waypoints in center-front lane + :param right_wps: waypoints in right-front lane + :param vehicle_inlane: six vehicles in left-front, center-front, right-front, left-rear, center-rear, right-rear + :return: + """ + self.left_wps = info_dict['left_wps'] + self.center_wps = info_dict['center_wps'] + self.right_wps = info_dict['right_wps'] + self.left_rear_wps = info_dict['left_rear_wps'] + self.center_rear_wps = info_dict['center_rear_wps'] + self.right_rear_wps = info_dict['right_rear_wps'] + self.distance_to_left_front = info_dict['vehs_info'].distance_to_front_vehicles[0] + self.distance_to_center_front = info_dict['vehs_info'].distance_to_front_vehicles[1] + self.distance_to_right_front = info_dict['vehs_info'].distance_to_front_vehicles[2] + self.distance_to_left_rear = info_dict['vehs_info'].distance_to_rear_vehicles[0] + self.distance_to_center_rear = info_dict['vehs_info'].distance_to_rear_vehicles[1] + self.distance_to_right_rear = info_dict['vehs_info'].distance_to_rear_vehicles[2] + self._vehicle_location = self._vehicle.get_location() + + print('the length of six waypoint queues: ', len(self.left_wps), len(self.center_wps), len(self.right_wps), + len(self.left_rear_wps), + len(self.center_rear_wps), len(self.right_rear_wps)) + # For simplicity, we compute s for front vehicles, and compute Euler distance for rear vehicles. + # set next waypoint that distance == 2m + # if len(self.left_wps) != 0: + # self.left_next_wayppoint = self.left_wps[1] + # if len(self.center_wps) != 0: + # self.center_next_waypoint = self.center_wps[1] + # if len(self.right_wps) != 0: + # self.right_next_waypoint = self.right_wps[1] + if self._ignore_change_gap: + if len(self.left_wps) != 0: + self.enable_left_change = True + if len(self.right_wps) != 0: + self.enable_right_change = True + else: + self.enable_left_change = False + self.enable_right_change = False + if len(self.left_wps) != 0: + self.enable_left_change = True + if len(self.right_wps) != 0: + self.enable_right_change = True + print("distance enable: ", self.distance_to_left_front, self.distance_to_center_front, + self.distance_to_right_front, self.distance_to_left_rear, self.distance_to_center_rear, + self.distance_to_right_rear, self.enable_left_change, self.enable_right_change) + + def run_step(self, current_lane, last_target_lane, last_action, modify_change_steer): + self.autopilot_step = self.autopilot_step + 1 + """Execute one step of navigation.""" + affected_by_tlight, affected_by_vehicle = False, False + # Retrieve all relevant actors + actor_list = self._world.get_actors() + vehicle_list = actor_list.filter("*vehicle*") + lights_list = actor_list.filter("*traffic_light*") + + vehicle_speed = get_speed(self._vehicle) / 3.6 + + # Check for possible vehicle obstacles + max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed + affected_by_vehicle = self._vehicle_obstacle_detected(max_vehicle_distance) + # Check if the vehicle is affected by a red traffic light + max_tlight_distance = self._base_tlight_threshold + vehicle_speed + affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) + + new_action, new_target_lane = self._lane_change_action(current_lane, last_target_lane, last_action) + + control = self._PID_run_step(new_action) + + if modify_change_steer: + if new_action == Action.LANE_CHANGE_LEFT: + control.steer = np.clip(control.steer, -1, 0) + elif new_action == Action.LANE_CHANGE_RIGHT: + control.steer = np.clip(control.steer, 0, 1) + if affected_by_tlight or (new_action == Action.LANE_FOLLOW and affected_by_vehicle): + control = self.add_emergency_stop(control) + + return control, new_target_lane, new_action + + def ignore_traffic_lights(self, active=True): + """(De)activates the checks for traffic lights""" + self._ignore_traffic_lights = active + + def ignore_stop_signs(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_stop_signs = active + + def ignore_vehicles(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_vehicles = active + + def get_step(self): + return self.autopilot_step + + def get_follow_action(self): + pass + + def _lane_change_action(self, current_lane, last_target_lane, last_action): + if self.random_lane_change: + if current_lane == -2: + lane_change = random.choice(self.center_random_change) + elif current_lane == -1: + lane_change = random.choice(self.left_random_change) + elif current_lane == -3: + lane_change = random.choice(self.right_random_change) + else: + # just to avoid error, dont work + lane_change = Action.LANE_FOLLOW + else: + if current_lane == -2: + change_choice = [] + if self.distance_to_left_front - self.distance_to_center_front > 5 and self.distance_to_left_rear > 1: + change_choice.append(Action.LANE_CHANGE_LEFT) + if self.distance_to_right_front - self.distance_to_center_front > 5 and self.distance_to_right_rear > 1: + change_choice.append(Action.LANE_CHANGE_RIGHT) + if len(change_choice) == 0: + lane_change = Action.LANE_FOLLOW + else: + lane_change = random.choice(change_choice) + elif current_lane == -1: + if self.distance_to_right_front - self.distance_to_center_front > 5 and self.distance_to_right_rear > 1: + lane_change = Action.LANE_CHANGE_RIGHT + else: + lane_change = Action.LANE_FOLLOW + elif current_lane == -3: + if self.distance_to_left_front - self.distance_to_center_front > 5 and self.distance_to_left_rear > 1: + lane_change = Action.LANE_CHANGE_LEFT + else: + lane_change = Action.LANE_FOLLOW + else: + # just to avoid error, dont work + lane_change = Action.LANE_FOLLOW + + if lane_change == Action.LANE_CHANGE_LEFT and not self.enable_left_change: + lane_change = Action.LANE_FOLLOW + if lane_change == Action.LANE_CHANGE_RIGHT and not self.enable_right_change: + lane_change = Action.LANE_FOLLOW + + if self.last_lane: + if current_lane == self.last_lane: + if self.lane_change_mode: + # still on last lane, change lane behavior not finish + new_action = last_action + new_target_lane = last_target_lane + else: + # lane follow mode + if lane_change != Action.LANE_FOLLOW: + self.lane_change_mode = True + new_action = lane_change + new_target_lane = current_lane - new_action.value + else: + # reach dest lane, change lane behavior finish + new_action = Action.LANE_FOLLOW + new_target_lane = current_lane + self.lane_change_mode = False + else: + self.lane_change_mode = False + new_action = Action.LANE_FOLLOW + new_target_lane = current_lane + self.last_lane = current_lane + + return new_action, new_target_lane + + def _PID_run_step(self, new_action): + """ + Execute one step of local planning which involves running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param debug: boolean flag to activate waypoints debugging + :return: control to be applied + """ + + # Purge the queue of obsolete waypoints + veh_location = self._vehicle.get_location() + veh_waypoint = get_lane_center(self._map, veh_location) + + vehicle_speed = get_speed(self._vehicle) / 3.6 + lane_center_ratio = 1 - veh_waypoint.transform.location.distance(veh_location) / 4 + self._min_distance = self._base_min_distance * lane_center_ratio + # print('min_distance: ', self._min_distance) + next_wp = 1 + if self._min_distance > 1: + next_wp = 2 + elif self._min_distance > 2: + next_wp = 3 + elif self._min_distance > 3: + next_wp = 4 + if new_action == Action.LANE_CHANGE_LEFT: + target_speed = 50 + self.target_waypoint = self.left_wps[next_wp + 20 - 1] + # print('left target waypoint: ', self.target_waypoint) + elif new_action == Action.LANE_FOLLOW: + target_speed = self._target_speed + self.target_waypoint = self.center_wps[next_wp + 5 - 1] + # print('center target waypoint: ', self.target_waypoint) + elif new_action == Action.LANE_CHANGE_RIGHT: + target_speed = 50 + self.target_waypoint = self.right_wps[next_wp + 20 - 1] + # print('right target waypoint: ', self.target_waypoint) + + # print("current location and target location: ", veh_location, self.target_waypoint.transform.location) + control = self._vehicle_controller.run_step(target_speed, self.target_waypoint) + + return ControlInfo(throttle=control.throttle, brake=control.brake, steer=control.steer, gear=control.gear) + + def _vehicle_obstacle_detected(self, max_dis): + have_dangerous_vehicle = False + if self.distance_to_center_front < max_dis and not self._ignore_vehicle: + have_dangerous_vehicle = True + + return have_dangerous_vehicle + + def _affected_by_traffic_light(self, lights_list=None, max_distance=None): + """ + Method to check if there is a red light affecting the vehicle. + + :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. + If None, all traffic lights in the scene are used + :param max_distance (float): max distance for traffic lights to be considered relevant. + If None, the base threshold value is used + """ + if self._ignore_traffic_lights: + return (False, None) + + if not lights_list: + lights_list = self._world.get_actors().filter("*traffic_light*") + + if not max_distance: + max_distance = self._base_tlight_threshold + + if self._last_traffic_light: + if self._last_traffic_light.state != carla.TrafficLightState.Red: + self._last_traffic_light = None + else: + return (True, self._last_traffic_light) + + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + for traffic_light in lights_list: + object_location = get_trafficlight_trigger_location(traffic_light) + object_waypoint = self._map.get_waypoint(object_location) + + if object_waypoint.road_id != ego_vehicle_waypoint.road_id: + continue + + ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() + wp_dir = object_waypoint.transform.get_forward_vector() + dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z + + if dot_ve_wp < 0: + continue + + if traffic_light.state != carla.TrafficLightState.Red: + continue + + if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): + self._last_traffic_light = traffic_light + return (True, traffic_light) + + return (False, None) \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_agent.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_agent.py new file mode 100644 index 0000000000..abe47f3a0e --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_agent.py @@ -0,0 +1,309 @@ +import carla +import numpy as np +from shapely.geometry import Polygon +from gym_carla.multi_lane.util.misc import get_speed,positive +from gym_carla.multi_lane.agent.basic_agent import BasicAgent +from gym_carla.multi_lane.agent.behavior_types import Cautious,Normal,Aggressive +from gym_carla.multi_lane.agent.local_planner import RoadOption +# +class BehaviorAgent(BasicAgent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment such as tailgating avoidance. + Adding to these are possible behaviors, the agent can also keep safety distance + from a car in front of it by tracking the instantaneous time to collision + and keeping it in a certain range. Finally, different sets of behaviors + are encoded in the agent, from cautious to a more aggressive ones. + """ + + def __init__(self, vehicle, behavior='normal'): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param ignore_traffic_light: boolean to ignore any traffic light + :param behavior: type of agent to apply + """ + + super(BehaviorAgent, self).__init__(vehicle) + self._look_ahead_steps = 0 + + # Vehicle information + self._speed = 0 + self._speed_limit = 0 + self._direction = None + self._incoming_direction = None + self._incoming_waypoint = None + self._min_speed = 5 + self._behavior = None + self._sampling_resolution = 4.5 + + # Parameters for agent behavior + if behavior == 'cautious': + self._behavior = Cautious() + + elif behavior == 'normal': + self._behavior = Normal() + + elif behavior == 'aggressive': + self._behavior = Aggressive() + + def run_step(self, next_waypoint): + """ + Execute one step of navigation. + + :param next_waypoint: next waypoint for vehicle + :return control: carla.VehicleControl + """ + self._update_information() + + control = None + if self._behavior.tailgate_counter > 0: + self._behavior.tailgate_counter -= 1 + + ego_vehicle_loc = self._vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + #1: Red lights and stops behavior + if self.traffic_light_manager(): + return self.emergency_stop() + + # 2.1: Pedestrian avoidance behaviors + walker_state, walker, w_distance = self.pedestrian_avoid_manager(ego_vehicle_wp) + + if walker_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = w_distance - max( + walker.bounding_box.extent.y, walker.bounding_box.extent.x) - max( + self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) + + # Emergency brake if the car is very close. + if distance < self._behavior.braking_distance: + return self.emergency_stop() + + # 2.2: Car following behaviors + vehicle_state, vehicle, distance = self.collision_and_car_avoid_manager(ego_vehicle_wp) + + if vehicle_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = distance - max( + vehicle.bounding_box.extent.y, vehicle.bounding_box.extent.x) - max( + self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) + + # Emergency brake if the car is very close. + if distance < self._behavior.braking_distance: + return self.emergency_stop() + else: + control = self.car_following_manager(vehicle, distance) + + # 3: Intersection behavior + elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - 5]) + self.set_target_speed(target_speed) + control=self._vehicle_controller.run_step(self._target_speed,next_waypoint) + + # 4: Normal behavior + else: + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self.set_target_speed(target_speed) + control=self._vehicle_controller.run_step(self._target_speed,next_waypoint) + + return control + + + def _update_information(self): + """ + This method updates the information regarding the ego + vehicle based on the surrounding world. + """ + self._speed = get_speed(self._vehicle) + self._speed_limit = self._vehicle.get_speed_limit() + self.set_target_speed(self._speed_limit) + self._direction = self._local_planner.target_road_option + if self._direction is None: + self._direction = RoadOption.LANEFOLLOW + + self._look_ahead_steps = int((self._speed_limit) / 10) + + self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self._look_ahead_steps) + if self._incoming_direction is None: + self._incoming_direction = RoadOption.LANEFOLLOW + + def traffic_light_manager(self): + """ + This method is in charge of behaviors for red lights. + """ + actor_list = self._world.get_actors() + lights_list = actor_list.filter("*traffic_light*") + affected, _ = self._affected_by_traffic_light(lights_list) + + return affected + + def _tailgating(self, waypoint, vehicle_list): + """ + This method is in charge of tailgating behaviors. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :param vehicle_list: list of all the nearby vehicles + """ + + left_turn = waypoint.left_lane_marking.lane_change + right_turn = waypoint.right_lane_marking.lane_change + + left_wpt = waypoint.get_left_lane() + right_wpt = waypoint.get_right_lane() + + behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, low_angle_th=160) + if behind_vehicle_state and self._speed < get_speed(behind_vehicle): + if (right_turn == carla.LaneChange.Right or right_turn == + carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: + new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + if not new_vehicle_state: + print("Tailgating, moving to the right!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.tailgate_counter = 200 + self.set_destination(end_waypoint.transform.location, + right_wpt.transform.location) + elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: + new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + if not new_vehicle_state: + print("Tailgating, moving to the left!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.tailgate_counter = 200 + self.set_destination(end_waypoint.transform.location, + left_wpt.transform.location) + + def collision_and_car_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + and managing possible tailgating chances. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a vehicle nearby, False if not + :return vehicle: nearby vehicle + :return distance: distance to nearby vehicle + """ + + vehicle_list = self._world.get_actors().filter("*vehicle*") + def dist(v): return v.get_location().distance(waypoint.transform.location) + vehicle_list = [v for v in vehicle_list if dist(v) < 45 and v.id != self._vehicle.id] + + if self._direction == RoadOption.CHANGELANELEFT: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + elif self._direction == RoadOption.CHANGELANERIGHT: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + else: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=30) + + # Check for tailgating + if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ + and not waypoint.is_junction and self._speed > 10 \ + and self._behavior.tailgate_counter == 0: + self._tailgating(waypoint, vehicle_list) + + return vehicle_state, vehicle, distance + + def pedestrian_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + with any pedestrian. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a walker nearby, False if not + :return vehicle: nearby walker + :return distance: distance to nearby walker + """ + + walker_list = self._world.get_actors().filter("*walker.pedestrian*") + def dist(w): return w.get_location().distance(waypoint.transform.location) + walker_list = [w for w in walker_list if dist(w) < 10] + + if self._direction == RoadOption.CHANGELANELEFT: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=-1) + elif self._direction == RoadOption.CHANGELANERIGHT: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=1) + else: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=60) + + return walker_state, walker, distance + + def car_following_manager(self, vehicle, distance, debug=False): + """ + Module in charge of car-following behaviors when there's + someone in front of us. + + :param vehicle: car to follow + :param distance: distance from vehicle + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + + vehicle_speed = get_speed(vehicle) + delta_v = max(1, (self._speed - vehicle_speed) / 3.6) + ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) + + # Under safety time distance, slow down. + if self._behavior.safety_time > ttc > 0.0: + target_speed = min([ + positive(vehicle_speed - self._behavior.speed_decrease), + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + # Actual safety distance area, try to follow the speed of the vehicle in front. + elif 2 * self._behavior.safety_time > ttc >= self._behavior.safety_time: + target_speed = min([ + max(self._min_speed, vehicle_speed), + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + # Normal behavior. + else: + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + return control + + def emergency_stop(self): + """ + Overwrites the throttle a brake values of a control to perform an emergency stop. + The steering is kept the same to avoid going out of the lane when stopping during turns + + :param speed (carl.VehicleControl): control to be modified + """ + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_types.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_types.py new file mode 100644 index 0000000000..a089c64ded --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/behavior_types.py @@ -0,0 +1,42 @@ +# +class Cautious(object): + """Class for Cautious agent.""" + max_speed = 40 + speed_lim_dist = 6 + speed_decrease = 12 + safety_time = 3 + min_proximity_threshold = 12 + braking_distance = 6 + tailgate_counter = 0 + + +class Normal(object): + """Class for Normal agent.""" + max_speed = 50 + speed_lim_dist = 3 + speed_decrease = 10 + safety_time = 3 + min_proximity_threshold = 10 + braking_distance = 5 + tailgate_counter = 0 + + +# class Aggressive(object): +# """Class for Aggressive agent.""" +# max_speed = 70 +# speed_lim_dist = 1 +# speed_decrease = 8 +# safety_time = 3 +# min_proximity_threshold = 8 +# braking_distance = 4 +# tailgate_counter = -1 + +class Aggressive(object): + """Class for Aggressive agent.""" + max_speed = 100 + speed_lim_dist = 0 + speed_decrease = 100 + safety_time = 0 + min_proximity_threshold = 0 + braking_distance = 0 + tailgate_counter = -1 \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/global_planner.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/global_planner.py new file mode 100644 index 0000000000..bbfa2d2ee8 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/global_planner.py @@ -0,0 +1,231 @@ +import carla +import logging, random +import numpy as np +import networkx as nx +import matplotlib.pyplot as plt +from enum import Enum +from gym_carla.multi_lane.settings import ROADS, STRAIGHT, CURVE, JUNCTION, DOUBLE_DIRECTION, DISTURB_ROADS +from gym_carla.multi_lane.util.misc import vector +# +class RoadOption(Enum): + """ + RoadOption represents the possible topological configurations when moving from a segment of lane to other. + + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + +def test_waypoint(wp): + """Attention: the test_waypoint here should be different from test_waypoint function in misc.py + or it could cause endless loop in GlobalPlanner._build_route()""" + return wp.road_id in STRAIGHT or wp.road_id in CURVE or wp.road_id in JUNCTION + +class GlobalPlanner: + """ + class for generating chosen circuit's road topology,topology is saved with waypoints list + vehicle always runs on the outer ring of chosen route + + temporarily used to get more spawnpoints + """ + + def __init__(self, map, sampling_resolution=1000.0) -> None: + self._sampling_resolution = sampling_resolution + self._wmap = map + + # code for simulation road generation + self._route = [] + self._topology = [] + + # generate circuit topology + self._build_topology() + # print(len(self._topology)) + # self._build_graph() + # nx.draw(self._graph,with_labels=True,font_weight='bold') + # plt.draw() + # plt.show() + + # generate route waypoints list + self._build_route() + + def get_route(self, ego_waypoint): + return self._compute_next_waypoints(ego_waypoint, len(self._route)) + + def get_spawn_points(self): + """Vehicle can only be spawned on specific roads, return transforms""" + spawn_points = [] + for wp in self._route: + # print('wp.lane_id: ', wp.lane_id) + if wp.road_id in STRAIGHT or wp.road_id in CURVE: + # print(wp.lane_id) + temp = carla.Transform(wp.transform.location, wp.transform.rotation) + # Increase the z value a little bit to avoid collison upon initializing + temp.location.z += 0.1 + spawn_points.append(temp) + + return spawn_points + + def split_spawn_points(self, spawn_points): + pass + + def _build_route(self): + begin_1 = self._topology[0] + begin_2 = self._topology[1] + begin_3 = self._topology[2] + self._build(begin_1) + self._build(begin_2) + self._build(begin_3) + + # remove start + # print(len(self._route)) + + def _build(self,begin): + self._route.append(begin['entry']) + for wp in begin['path']: + self._route.append(wp) + # self._route.append(begin['exit']) + indicator = begin['exit'] + iter = None + for seg in self._topology: + if seg['entry'].id == indicator.id: + iter = seg + break + + while (indicator.id != begin['entry'].id): + self._route.append(iter['entry']) + for wp in iter['path']: + self._route.append(wp) + # self._route.append(iter['exit']) + indicator = iter['exit'] + for seg in self._topology: + if seg['entry'].id == indicator.id: + iter = seg + break + + def _compute_next_waypoints(self, cur_wp, k=1): + """ + Add new waypoints to the trajectory queue. + + :param cur_wp: current waypoint + :param k: how many waypoints to compute + :return: waypoint list + """ + next_wps = [] + iter = None + for i, wp in enumerate(self._route): + if wp.id == cur_wp.id: + iter = i + break + elif wp.transform.location.distance(cur_wp.transform.location) < self._sampling_resolution / 2: + # can't find the exact waypoint, get an approximation + iter = i + if iter is None: + logging.error("Current waypoint on route not found!") + if iter + k < len(self._route): + for i in range(k): + next_wps.append(self._route[iter + i + 1]) + else: + for i in range(len(self._route) - iter - 1): + next_wps.append(self._route[iter + i + 1]) + for i in range(k - (len(self._route) - iter - 1)): + next_wps.append(self._route[i]) + + return next_wps + + def _build_topology(self): + """ + This function retrieves topology from the server as a list of + road segments as pairs of waypoint objects, and processes the + topology into a list of dictionary objects with the following attributes + + - entry (carla.Waypoint): waypoint of entry point of road segment + - entryxyz (tuple): (x,y,z) of entry point of road segment + - exit (carla.Waypoint): waypoint of exit point of road segment + - exitxyz (tuple): (x,y,z) of exit point of road segment + - path (list of carla.Waypoint): list of waypoints between entry to exit, separated by the resolution + """ + # Retrieving waypoints to construct a detailed topology + for segment in self._wmap.get_topology(): + wp1, wp2 = segment[0], segment[1] + if test_waypoint(wp1) and test_waypoint(wp2): + l1, l2 = wp1.transform.location, wp2.transform.location + # Rounding off to avoid floating point imprecision + x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0) + wp1.transform.location, wp2.transform.location = l1, l2 + seg_dict = dict() + seg_dict['entry'], seg_dict['exit'] = wp1, wp2 + seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2) + seg_dict['path'] = [] + endloc = wp2.transform.location + if wp1.transform.location.distance(endloc) > self._sampling_resolution: + w = wp1.next(self._sampling_resolution)[0] + while w.transform.location.distance(endloc) > self._sampling_resolution: + if test_waypoint(w): + seg_dict['path'].append(w) + w = w.next(self._sampling_resolution)[0] + if test_waypoint(w): + seg_dict['path'].append(w) + else: + next_wp = wp1.next(self._sampling_resolution)[0] + if test_waypoint(next_wp): + seg_dict['path'].append(next_wp) + self._topology.append(seg_dict) + + def _build_graph(self): + """ + This function builds a networkx graph representation of topology, creating several class attributes: + - graph (networkx.DiGraph): networkx graph representing the world map, with: + Node properties: + vertex: (x,y,z) position in world map + Edge properties: + entry_vector: unit vector along tangent at entry point + exit_vector: unit vector along tangent at exit point + net_vector: unit vector of the chord from entry to exit + intersection: boolean indicating if the edge belongs to an intersection + - id_map (dictionary): mapping from (x,y,z) to node id + - road_id_to_edge (dictionary): map from road id to edge in the graph + """ + + self._graph = nx.DiGraph() + self._id_map = dict() # Map with structure {(x,y,z): id, ... } + self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... } + + for segment in self._topology: + entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz'] + path = segment['path'] + entry_wp, exit_wp = segment['entry'], segment['exit'] + intersection = entry_wp.is_junction + road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id + + for vertex in entry_xyz, exit_xyz: + # Adding unique nodes and populating id_map + if vertex not in self._id_map: + new_id = len(self._id_map) + self._id_map[vertex] = new_id + self._graph.add_node(new_id, vertex=vertex) + n1 = self._id_map[entry_xyz] + n2 = self._id_map[exit_xyz] + if road_id not in self._road_id_to_edge: + self._road_id_to_edge[road_id] = dict() + if section_id not in self._road_id_to_edge[road_id]: + self._road_id_to_edge[road_id][section_id] = dict() + self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) + + entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() + exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() + + # Adding edge with attributes + self._graph.add_edge( + n1, n2, + length=len(path) + 1, path=path, + entry_waypoint=entry_wp, exit_waypoint=exit_wp, + entry_vector=np.array( + [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]), + exit_vector=np.array( + [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]), + net_vector=vector(entry_wp.transform.location, exit_wp.transform.location), + intersection=intersection, type=RoadOption.LANEFOLLOW) diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/local_planner.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/local_planner.py new file mode 100644 index 0000000000..e2191f811d --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/local_planner.py @@ -0,0 +1,531 @@ +import carla +import copy +import logging +from collections import deque +from shapely.geometry import Polygon +from gym_carla.multi_lane.agent.global_planner import RoadOption +from gym_carla.multi_lane.util.wrapper import WaypointWrapper, VehicleWrapper +from gym_carla.multi_lane.settings import ROADS, STRAIGHT, CURVE, JUNCTION, DOUBLE_DIRECTION, DISTURB_ROADS +from gym_carla.multi_lane.util.misc import get_lane_center, get_speed, vector, compute_magnitude_angle, \ + is_within_distance_ahead, is_within_distance_rear, draw_waypoints, compute_distance, is_within_distance, \ + test_waypoint, \ + get_trafficlight_trigger_location + + +# +class LocalPlanner: + def __init__(self, vehicle, + opt_dict={'sampling_resolution': 4.0, + 'buffer_size': 10, + 'vehicle_proximity': 50}): + """ + temporarily used to get front waypoints and vehicle + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + + self._sampling_radius = opt_dict['sampling_resolution'] + self._base_min_distance = 3.0 # This value is tricky + + self._target_waypoint = None + self._buffer_size = opt_dict['buffer_size'] + self._waypoint_buffer = deque(maxlen=self._buffer_size) + + self._waypoints_queue = deque(maxlen=600) + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + self._target_road_option = RoadOption.LANEFOLLOW + self._stop_waypoint_creation = False + + self.vehicle_proximity = opt_dict['vehicle_proximity'] + self.traffic_light_proximity = opt_dict['traffic_light_proximity'] + + self.waypoints_info = None + self.lights_info = None + self.vehicles_info = None + + self._waypoints_queue.append((self._current_waypoint, RoadOption.LANEFOLLOW)) + # self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) + # self._compute_next_waypoints(k=200) + + def run_step(self): + self.waypoints_info = self._get_waypoints() + self.lights_info = self._get_traffic_lights() + self.vehicles_info = self._get_vehicles() + + return WaypointWrapper(self.waypoints_info), self.lights_info, VehicleWrapper(self.vehicles_info) + + # def _get_traffic_lights(self): + # lights_list = self._world.get_actors().filter("*traffic_light*") + # max_distance = self.traffic_light_proximity + # ego_vehicle_location = self._vehicle.get_location() + # ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + # sel_traffic_light = None + + # if ego_vehicle_waypoint.is_junction: + # # It is too late. Do not block the intersection! Keep going! + # return sel_traffic_light + + # # if self._last_traffic_light: + # # if self._last_traffic_light.state != carla.TrafficLightState.Red: + # # self._last_traffic_light = None + # # else: + # # return self._last_traffic_light + + # for traffic_light in lights_list: + # object_location = get_trafficlight_trigger_location(traffic_light) + # object_waypoint = self._map.get_waypoint(object_location) + + # if object_waypoint.road_id != ego_vehicle_waypoint.road_id: + # continue + + # ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() + # wp_dir = object_waypoint.transform.get_forward_vector() + # dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z + + # if dot_ve_wp < 0: + # continue + + # # if traffic_light.state != carla.TrafficLightState.Red: + # # continue + + # if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): + # sel_traffic_light = traffic_light + # self._world.debug.draw_box(traffic_light.trigger_volume,traffic_light.trigger_volume.rotation,life_time=0) + # return sel_traffic_light + + # return sel_traffic_light + + def _get_traffic_lights(self): + """ + This method is specialized to check US style traffic lights. + + :param lights_list: list containing TrafficLight objects + :return: a tuple given by (bool_flag, traffic_light), where + - bool_flag is True if there is a traffic light in RED + affecting us and False otherwise + - traffic_light is the object itself or None if there is no + red traffic light affecting us + """ + lights_list = self._world.get_actors().filter("*traffic_light*") + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + sel_traffic_light = None + + if ego_vehicle_waypoint.is_junction: + # It is too late. Do not block the intersection! Keep going! + return sel_traffic_light + + for traffic_light in lights_list: + wps = traffic_light.get_stop_waypoints() + for wp in wps: + if wp.road_id == ego_vehicle_waypoint.road_id: + if wp.lane_id == ego_vehicle_waypoint.lane_id and \ + wp.transform.location.distance(ego_vehicle_location) <= self.traffic_light_proximity: + sel_traffic_light = traffic_light + return sel_traffic_light + + return sel_traffic_light + + def _get_vehicles(self): + # retrieve relevant elements for safe navigation, i.e.: other vehicles + def caculate_dis(wps, veh): + if len(wps) == 0: + return 0 + else: + if veh: + pre_wps = wps[0].previous(self._sampling_radius) + pre_wp = None + if len(pre_wps) == 1: + pre_wp = pre_wps[0] + elif len(pre_wps) != 0: + for i, wp in enumerate(pre_wps): + if wp.road_id in ROADS: + pre_wp = wp + vehicle_len = max(abs(self._vehicle.bounding_box.extent.x), + abs(self._vehicle.bounding_box.extent.y)) + \ + max(abs(veh.bounding_box.extent.x), + abs(veh.bounding_box.extent.y)) + + return max(pre_wp.transform.location.distance(veh.get_location()) - vehicle_len, 0.0001) + else: + return self.vehicle_proximity + + vehicle_list = self._world.get_actors().filter("*vehicle*") + left_front_veh = self._get_vehicles_one_lane(vehicle_list, True, -1) + left_rear_veh = self._get_vehicles_one_lane(vehicle_list, False, -1) + center_front_veh = self._get_vehicles_one_lane(vehicle_list, True, 0) + center_rear_veh = self._get_vehicles_one_lane(vehicle_list, False, 0) + right_front_veh = self._get_vehicles_one_lane(vehicle_list, True, 1) + right_rear_veh = self._get_vehicles_one_lane(vehicle_list, False, 1) + + distance_to_front_vehicles = [] + distance_to_rear_vehicles = [] + distance_to_front_vehicles.append(caculate_dis(self.waypoints_info['left_front_wps'], left_front_veh)) + distance_to_front_vehicles.append(caculate_dis(self.waypoints_info['center_front_wps'], center_front_veh)) + distance_to_front_vehicles.append(caculate_dis(self.waypoints_info['right_front_wps'], right_front_veh)) + distance_to_rear_vehicles.append(caculate_dis(self.waypoints_info['left_rear_wps'], left_rear_veh)) + distance_to_rear_vehicles.append(caculate_dis(self.waypoints_info['center_rear_wps'], center_rear_veh)) + distance_to_rear_vehicles.append(caculate_dis(self.waypoints_info['right_rear_wps'], right_rear_veh)) + + return {'left_front_veh': left_front_veh, + 'left_rear_veh': left_rear_veh, + 'center_front_veh': center_front_veh, + 'center_rear_veh': center_rear_veh, + 'right_front_veh': right_front_veh, + 'right_rear_veh': right_rear_veh, + 'dis_to_front_vehs': distance_to_front_vehicles, + 'dis_to_rear_vehs': distance_to_rear_vehicles} + + def _get_vehicles_one_lane(self, vehicle_list, direction=True, lane_offset=0): + """ + Check if a given vehicle is an obstacle in our way. To this end we take + into account the road and lane the target vehicle is on and run a + geometry test to check if the target vehicle is under a certain distance + behind our ego vehicle. + + WARNING: This method is an approximation that could fail for very large + vehicles, which center is actually on a different lane but their + extension falls within the ego vehicle lane. + + :param vehicle_list: list of potential obstacle to check + :param direction: True--detect vehicles in front of ego vehicle + False--detec vehicles at the back of ego vehicle + :param lane_offset: the lane relative to current ego vehicle's lane, + minus value means left, positive value means right + """ + + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_transform = self._vehicle.get_transform() + ego_vehicle_lane_center = get_lane_center(self._map, ego_vehicle_location) + if not test_waypoint(ego_vehicle_lane_center): + return None + + min_distance = self.vehicle_proximity + vehicle = None + lane_id = ego_vehicle_lane_center.lane_id - lane_offset + if lane_id != -1 and lane_id != -2 and lane_id != -3: + return vehicle + + for target_vehicle in vehicle_list: + # do not account for the ego vehicle + if target_vehicle.id == self._vehicle.id: + continue + + # if the object is not in our lane it's not an obstacle + target_vehicle_waypoint = self._map.get_waypoint(target_vehicle.get_location()) + # check whether in the same road + target_lane_center = get_lane_center(self._map, target_vehicle.get_location()) + if target_lane_center.transform.location.distance( + target_vehicle.get_location()) > target_lane_center.lane_width / 2 + 0.1: + continue + if not test_waypoint(target_vehicle_waypoint): + continue + # check whether in the specific lane + if target_vehicle_waypoint.lane_id != lane_id: + continue + # if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \ + # target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id: + # continue + + loc = target_vehicle.get_location() + if direction: + if is_within_distance_ahead(loc, ego_vehicle_location, ego_vehicle_transform, self.vehicle_proximity): + if ego_vehicle_location.distance(loc) < min_distance: + # Return the most close vehicel in front of ego vehicle + vehicle = target_vehicle + min_distance = ego_vehicle_location.distance(loc) + else: + if is_within_distance_rear(loc, ego_vehicle_location, ego_vehicle_transform, self.vehicle_proximity): + if ego_vehicle_location.distance(loc) < min_distance: + # Return the most close vehicel in front of ego vehicle + vehicle = target_vehicle + min_distance = ego_vehicle_location.distance(loc) + + return vehicle + + def _get_waypoints(self): + left_front_wps = None + left_rear_wps = None + center_front_wps = None + center_rear_wps = None + right_front_wps = None + right_rear_wps = None + + lane_center = get_lane_center(self._map, self._vehicle.get_location()) + lane_id = lane_center.lane_id + left = None + center = lane_center + right = None + if lane_id == -1: + right = center.get_right_lane() + elif lane_id == -2: + left = center.get_left_lane() + right = center.get_right_lane() + elif lane_id == -3: + left = center.get_left_lane() + else: + lane_center = None + # logging.error("WAYPOINTS GET BUG") + + left_front_wps = self._get_waypoints_one_lane(left, True) + left_rear_wps = self._get_waypoints_one_lane(left, False) + center_front_wps = self._get_waypoints_one_lane(center, True) + center_rear_wps = self._get_waypoints_one_lane(center, False) + right_front_wps = self._get_waypoints_one_lane(right, True) + right_rear_wps = self._get_waypoints_one_lane(right, False) + + return {'left_front_wps': list(left_front_wps), + 'left_rear_wps': list(left_rear_wps), + 'center_front_wps': list(center_front_wps), + 'center_rear_wps': list(center_rear_wps), + 'right_front_wps': list(right_front_wps), + 'right_rear_wps': list(right_rear_wps)} + + def _get_waypoints_one_lane(self, waypoint=None, direction=True): + """Get the waypoint list according to ego vehicle's current location, + direction = True: caculated waypoints in front of current location, + direction = False: caculated waypoints at the back of current location""" + _waypoints_queue = deque(maxlen=600) + if waypoint is not None: + _waypoints_queue.append(waypoint) + available_entries = _waypoints_queue.maxlen - len(self._waypoints_queue) + k = min(available_entries, self._buffer_size) + for _ in range(k): + last_waypoint = _waypoints_queue[-1] + if direction: + next_waypoints = list(last_waypoint.next(self._sampling_radius)) + else: + next_waypoints = list(last_waypoint.previous(self._sampling_radius)) + + if len(next_waypoints) == 0: + break + elif len(next_waypoints) == 1: + # only one option available ==> lanefollowing + next_waypoint = next_waypoints[0] + # road_option = RoadOption.LANEFOLLOW + else: + # road_options_list = self._retrieve_options( + # next_waypoints, last_waypoint) + + idx = None + for i, wp in enumerate(next_waypoints): + if wp.road_id in ROADS: + next_waypoint = wp + idx = i + # road_option = road_options_list[idx] + + _waypoints_queue.append(next_waypoint) + # delete an element from the left + _waypoints_queue.popleft() + return list(_waypoints_queue) + + # def _get_waypoints(self): + # """ + # Execute one step of local planning which involves running the longitudinal and lateral PID controllers to + # follow the waypoints trajectory. + + # :param debug: boolean flag to activate waypoints debugging + # :return: + # """ + + # # not enough waypoints in the horizon? => add more! + # if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5) and not self._stop_waypoint_creation: + # self._compute_next_waypoints(self._buffer_size * 2) + + # # Buffering the waypoints + # while len(self._waypoint_buffer) < self._buffer_size: + # if self._waypoints_queue: + # self._waypoint_buffer.append( + # self._waypoints_queue.popleft()) + # else: + # break + + # waypoints = [] + + # for i, (waypoint, _) in enumerate(self._waypoint_buffer): + # waypoints.append(waypoint) + # # waypoints.append([waypoint.transform.location.x, waypoint.transform.location.y, waypoint.transform.rotation.yaw]) + + # # current vehicle waypoint + # self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + # # target waypoint + # self._target_waypoint, self._target_road_option = self._waypoint_buffer[0] + + # # purge the queue of obsolete waypoints + # # vehicle_transform = self._vehicle.get_transform() + # # max_index = -1 + + # # for i, (waypoint, _) in enumerate(self._waypoint_buffer): + # # if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: + # # max_index = i + # # if max_index >= 0: + # # for i in range(max_index - 1): + # # self._waypoint_buffer.popleft() + + # veh_location = self._vehicle.get_location() + # veh_speed = get_speed(self._vehicle, False) + # settings = self._world.get_settings() + # if settings.synchronous_mode: + # self._min_distance = self._base_min_distance + settings.fixed_delta_seconds * veh_speed + # else: + # self._min_distance = self._base_min_distance + 0.5 * veh_speed + # num_waypoint_removed = 0 + # for waypoint, _ in self._waypoint_buffer: + + # if len(self._waypoints_queue) - num_waypoint_removed == 1: + # min_distance = 1 # Don't remove the last waypoint until very close by + # else: + # min_distance = self._min_distance + + # if veh_location.distance(waypoint.transform.location) < min_distance: + # num_waypoint_removed += 1 + # else: + # break + + # if num_waypoint_removed > 0: + # for _ in range(num_waypoint_removed): + # self._waypoint_buffer.popleft() + + # # lane_center=get_lane_center(self._map,self._vehicle.get_location()) + # # print(lane_center.road_id,lane_center.lane_id,lane_center.s,sep='\t',end='\n\n') + # # for wp,_ in self._waypoint_buffer: + # # print(wp.road_id,wp.lane_id,wp.s,wp.transform.location.distance(lane_center.transform.location),sep='\t') + + # return waypoints + + def _retrieve_options(self, list_waypoints, current_waypoint): + """ + Compute the type of connection between the current active waypoint and the multiple waypoints present in + list_waypoints. The result is encoded as a list of RoadOption enums. + + :param list_waypoints: list with the possible target waypoints in case of multiple options + :param current_waypoint: current active waypoint + :return: list of RoadOption enums representing the type of connection from the active waypoint to each + candidate in list_waypoints + """ + options = [] + for next_waypoint in list_waypoints: + # this is needed because something we are linking to + # the beggining of an intersection, therefore the + # variation in angle is small + next_next_waypoint = next_waypoint.next(3.0)[0] + link = self._compute_connection(current_waypoint, next_next_waypoint) + options.append(link) + + return options + + def _compute_connection(self, current_waypoint, next_waypoint): + """ + Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint + (next_waypoint). + + :param current_waypoint: active waypoint + :param next_waypoint: target waypoint + :return: the type of topological connection encoded as a RoadOption enum: + RoadOption.STRAIGHT + RoadOption.LEFT + RoadOption.RIGHT + """ + n = next_waypoint.transform.rotation.yaw + n = n % 360.0 + + c = current_waypoint.transform.rotation.yaw + c = c % 360.0 + + diff_angle = (n - c) % 180.0 + if diff_angle < 1.0: + return RoadOption.STRAIGHT + elif diff_angle > 90.0: + return RoadOption.LEFT + else: + return RoadOption.RIGHT + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self._waypoint_buffer) > steps: + return self._waypoint_buffer[steps] + else: + try: + wpt, direction = self._waypoint_buffer[-1] + return wpt, direction + except IndexError as i: + return None, RoadOption.VOID + + def set_sampling_redius(self, sampling_resolution): + self._sampling_radius = sampling_resolution + + def set_min_distance(self, min_distance): + self._min_distance = min_distance + + def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): + """ + Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs + The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one + The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints + + :param current_plan: list of (carla.Waypoint, RoadOption) + :param stop_waypoint_creation: bool + :param clean_queue: bool + :return: + """ + if clean_queue: + self._waypoints_queue.clear() + + # Remake the waypoints queue if the new plan has a higher length than the queue + new_plan_length = len(current_plan) + len(self._waypoints_queue) + if new_plan_length > self._waypoints_queue.maxlen: + new_waypoint_queue = deque(maxlen=new_plan_length) + for wp in self._waypoints_queue: + new_waypoint_queue.append(wp) + self._waypoints_queue = new_waypoint_queue + + for elem in current_plan: + self._waypoints_queue.append((elem, RoadOption.LANEFOLLOW)) + + self._stop_waypoint_creation = stop_waypoint_creation + + def _compute_next_waypoints(self, k=1): + """ + Add new waypoints to the trajectory queue. + + :param k: how many waypoints to compute + :return: + """ + # check we do not overflow the queue + available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) + k = min(available_entries, k) + + for _ in range(k): + last_waypoint = self._waypoints_queue[-1][0] + next_waypoints = list(last_waypoint.next(self._sampling_radius)) + + if len(next_waypoints) == 0: + break + elif len(next_waypoints) == 1: + # only one option available ==> lanefollowing + next_waypoint = next_waypoints[0] + road_option = RoadOption.LANEFOLLOW + else: + road_options_list = self._retrieve_options( + next_waypoints, last_waypoint) + + # # random choice between the possible options + # road_option = road_options_list[1] + # #road_option = random.choice(road_options_list) + # next_waypoint = next_waypoints[road_options_list.index(road_option)] + + idx = None + for i, wp in enumerate(next_waypoints): + if wp.road_id in ROADS: + next_waypoint = wp + idx = i + road_option = road_options_list[idx] + + self._waypoints_queue.append((next_waypoint, road_option)) \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/pid_controller.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/pid_controller.py new file mode 100644 index 0000000000..095934d653 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/agent/pid_controller.py @@ -0,0 +1,252 @@ +""" This module contains PID controllers to perform lateral and longitudinal control. """ +import math +import numpy as np +import carla +from collections import deque +from gym_carla.multi_lane.util.misc import get_speed + +class VehiclePIDController(): + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + low level control a vehicle from client side + """ + + # + + def __init__(self, vehicle, args_lateral={}, args_longitudinal={}, offset=0, max_throttle=0.75, max_brake=0.3, + max_steering=0.8): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller + using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param offset: If different than zero, the vehicle will drive displaced from the center line. + Positive values imply a right offset while negative ones mean a left one. Numbers high enough + to cause the vehicle to drive through other lanes might break the controller. + """ + + self.max_brake = max_brake + self.max_throt = max_throttle + self.max_steer = max_steering + + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self.past_steering = self._vehicle.get_control().steer + self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) + self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral) + + def run_step(self, target_speed, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint + at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + + acceleration = self._lon_controller.run_step(target_speed) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + + def change_longitudinal_PID(self, args_longitudinal): + """Changes the parameters of the PIDLongitudinalController""" + self._lon_controller.change_parameters(**args_longitudinal) + + def change_lateral_PID(self, args_lateral): + """Changes the parameters of the PIDLongitudinalController""" + self._lon_controller.change_parameters(**args_lateral) + + +class PIDLongitudinalController(): + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :param dt: time differential in seconds + """ + self._vehicle = vehicle + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + self._error_buffer = deque(maxlen=10) + + def run_step(self, target_speed, debug=False): + """ + Execute one step of longitudinal control to reach a given target speed. + + :param target_speed: target speed in Km/h + :param debug: boolean for debugging + :return: throttle control + """ + current_speed = get_speed(self._vehicle) + + if debug: + print('Current speed = {}'.format(current_speed)) + + return self._pid_control(target_speed, current_speed) + + def _pid_control(self, target_speed, current_speed): + """ + Estimate the throttle/brake of the vehicle based on the PID equations + + :param target_speed: target speed in Km/h + :param current_speed: current speed of the vehicle in Km/h + :return: throttle/brake control + """ + + error = target_speed - current_speed + self._error_buffer.append(error) + + if len(self._error_buffer) >= 2: + _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt + _ie = sum(self._error_buffer) * self._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + def change_parameters(self, K_P, K_I, K_D, dt): + """Changes the PID parameters""" + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + + +class PIDLateralController(): + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param offset: distance to the center line. If might cause issues if the value + is large enough to make the vehicle invade other lanes. + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :param dt: time differential in seconds + """ + self._vehicle = vehicle + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + self._offset = offset + self._e_buffer = deque(maxlen=10) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the vehicle towards a certain waypoin. + + :param waypoint: target waypoint + :return: steering control in the range [-1, 1] where: + -1 maximum steering to left + +1 maximum steering to right + """ + return self._pid_control(waypoint, self._vehicle.get_transform()) + + def _pid_control(self, waypoint, vehicle_transform): + """ + Estimate the steering angle of the vehicle based on the PID equations + + :param waypoint: target waypoint + :param vehicle_transform: current transform of the vehicle + :return: steering control in the range [-1, 1] + """ + # Get the ego's location and forward vector + ego_loc = vehicle_transform.location + v_vec = vehicle_transform.get_forward_vector() + v_vec = np.array([v_vec.x, v_vec.y, 0.0]) + + # Get the vector vehicle-target_wp + if self._offset != 0: + # Displace the wp to the side + w_tran = waypoint.transform + r_vec = w_tran.get_right_vector() + w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x, + y=self._offset*r_vec.y) + else: + w_loc = waypoint.transform.location + + w_vec = np.array([w_loc.x - ego_loc.x, + w_loc.y - ego_loc.y, + 0.0]) + + wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec) + if wv_linalg == 0: + _dot = 1 + else: + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0)) + _cross = np.cross(v_vec, w_vec) + if _cross[2] < 0: + _dot *= -1.0 + + self._e_buffer.append(_dot) + if len(self._e_buffer) >= 2: + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt + _ie = sum(self._e_buffer) * self._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + def change_parameters(self, K_P, K_I, K_D, dt): + """Changes the PID parameters""" + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/carla_env.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/carla_env.py new file mode 100644 index 0000000000..688ad2ba63 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/carla_env.py @@ -0,0 +1,1185 @@ +import time +import carla +import random +import logging +import pygame +import math, copy +import numpy as np +from enum import Enum +from queue import Queue +from collections import deque +from gym_carla.multi_lane.util.render import World, HUD +from gym_carla.multi_lane.agent.basic_agent import BasicAgent +from gym_carla.multi_lane.agent.local_planner import LocalPlanner +from gym_carla.multi_lane.agent.global_planner import GlobalPlanner, RoadOption +from gym_carla.multi_lane.agent.basic_lanechanging_agent import Basic_Lanechanging_Agent +# from gym_carla.single_lane.navigation.constant_velocity_agent import ConstantVelocityAgent +from gym_carla.multi_lane.util.sensor import CollisionSensor, LaneInvasionSensor, SemanticTags +from gym_carla.multi_lane.util.wrapper import WaypointWrapper, VehicleWrapper, Action, SpeedState, Truncated, \ + ControlInfo, process_veh, \ + process_steer, recover_steer, fill_action_param, ttc_reward, comfort, lane_center_reward, \ + calculate_guide_lane_center, process_lane_wp +from gym_carla.multi_lane.util.misc import draw_waypoints, get_speed, get_acceleration, test_waypoint, \ + compute_distance, get_actor_polygons, get_lane_center, remove_unnecessary_objects, get_yaw_diff, \ + get_trafficlight_trigger_location, is_within_distance, get_sign, is_within_distance_ahead, get_projection, \ + create_vehicle_blueprint + + +# +class CarlaEnv: + def __init__(self, args) -> None: + super().__init__() + self.host = args.host + self.port = args.port + self.tm_port = args.tm_port + self.sync = args.sync + self.fps = args.fps + self.no_rendering = args.no_rendering + self.ego_filter = args.filter + self.loop = args.loop + self.agent = args.agent + # arguments for debug + self.debug = args.debug + self.train = args.train # argument indicating training agent + self.seed = args.seed + self.behavior = args.behavior + self.num_of_vehicles = args.num_of_vehicles + self.sampling_resolution = args.sampling_resolution + self.min_distance = args.min_distance + self.vehicle_proximity = args.vehicle_proximity + self.traffic_light_proximity = args.traffic_light_proximity + self.hybrid = args.hybrid + self.auto_lanechange = args.auto_lane_change + self.guide_change = args.guide_change + self.stride = args.stride + self.buffer_size = 160000 + if self.train: + self.pre_train_steps = args.pre_train_steps + else: + self.pre_train_steps = 0 + self.speed_limit = args.speed_limit + self.lane_change_reward = args.lane_change_reward + # The RL agent acts only after ego vehicle speed reach speed threshold + self.speed_threshold = args.speed_threshold + self.speed_min = args.speed_min + # controller action space + self.steer_bound = args.steer_bound + self.throttle_bound = args.throttle_bound + self.brake_bound = args.brake_bound + self.modify_change_steer = args.modify_change_steer + self.ignore_traffic_light = args.ignore_traffic_light + + logging.info('listening to server %s:%s', args.host, args.port) + self.client = carla.Client(self.host, self.port) + self.client.set_timeout(10.0) + self.sim_world = self.client.load_world(args.map) + remove_unnecessary_objects(self.sim_world) + self.map = self.sim_world.get_map() + self.origin_settings = self.sim_world.get_settings() + self.traffic_manager = None + self.speed_state = SpeedState.START + # Set fixed simulation step for synchronous mode + self._set_synchronous_mode() + self._set_traffic_manager() + logging.info('Carla server connected') + + # init pygame window + self.pygame = args.pygame and not self.no_rendering + self.width, self.height = [int(x) for x in args.res.split('x')] + if self.pygame: + self._init_renderer() + + # Record the time of total steps + self.reset_step = 0 + self.total_step = 0 + self.time_step = 0 + self.rl_control_step = 0 + # RL_switch: True--currently RL in control, False--currently PID in control + self.RL_switch = False + self.SWITCH_THRESHOLD = args.switch_threshold + self.switch_count = 0 + self.lights_info = None + self.last_light_state = None + self.wps_info = WaypointWrapper() + self.vehs_info = VehicleWrapper() + self.control = ControlInfo() + # self.control = carla.VehicleControl(throttle=0.0, steer=0.0, brake=0.0,reverse=False, manual_gear_shift=False, gear=1) + + self.last_lane, self.current_lane = None, None + self.last_action, self.current_action = Action.LANE_FOLLOW, Action.LANE_FOLLOW + self.last_target_lane, self.current_target_lane = None, None + + self.calculate_impact = None + + # generate ego vehicle spawn points on chosen route + self.global_planner = GlobalPlanner(self.map, self.sampling_resolution) + self.local_planner = None + self.spawn_points = self.global_planner.get_spawn_points() + + # arguments for caculating reward + self.TTC_THRESHOLD = args.TTC_th + self.penalty = args.penalty + self.lane_penalty = args.lane_penalty + self.last_acc = 0 # ego vehicle acceration along s in last step + self.last_yaw = carla.Vector3D() + self.vel_buffer = deque(maxlen=10) + self.rear_vel_deque = deque(maxlen=2) + self.step_info = None + + if self.debug: + # draw_waypoints(self.sim_world,self.global_panner.get_route()) + random.seed(self.seed) + + # Set weather + # self.sim_world.set_weather(carla.WeatherParamertes.ClearNoon) + + self.companion_vehicles = [] + self.vehicle_polygons = [] + self.ego_vehicle = None + self.ego_spawn_point = None + + # Collision sensor + self.collision_sensor = None + self.lane_invasion_sensor = None + + # thread blocker + self.sensor_queue = Queue(maxsize=10) + self.camera = None + + def __del__(self): + logging.info('\n Destroying all vehicles') + self.sim_world.apply_settings(self.origin_settings) + self._clear_actors(['vehicle.*', 'sensor.other.collison', 'sensor.camera.rgb', 'sensor.other.lane_invasion']) + + def reset(self): + if self.ego_vehicle is not None: + self._clear_actors( + ['*vehicle.*', 'sensor.other.collision', 'sensor.camera.rgb', 'sensor.other.lane_invasion']) + self.ego_vehicle = None + self.vehicle_polygons.clear() + self.companion_vehicles.clear() + self.collision_sensor = None + self.lane_invasion_sensor = None + self.camera = None + self.vel_buffer.clear() + self.step_info.clear() + while (self.sensor_queue.empty() is False): + self.sensor_queue.get(block=False) + if self.pygame: + self.world.destroy() + pygame.quit() + else: + self.step_info = {} + + # Spawn surrounding vehicles + self._spawn_companion_vehicles() + self.calculate_impact = 0 + self.rear_vel_deque.append(-1) + self.rear_vel_deque.append(-1) + # Get actors polygon list + vehicle_poly_dict = get_actor_polygons(self.sim_world, 'vehicle.*') + self.vehicle_polygons.append(vehicle_poly_dict) + + # try to spawn ego vehicle + while self.ego_vehicle is None: + self.ego_spawn_point = random.choice(self.spawn_points) + self.ego_vehicle = self._try_spawn_ego_vehicle_at(self.ego_spawn_point) + # self.ego_vehicle.set_simulate_physics(False) + self.collision_sensor = CollisionSensor(self.ego_vehicle) + self.lane_invasion_sensor = LaneInvasionSensor(self.ego_vehicle) + # friction_bp=self.sim_world.get_blueprint_library().find('static.trigger.friction') + # bb_extent=self.ego_vehicle.bounding_box.extent + # friction_bp.set_attribute('friction',str(0.0)) + # friction_bp.set_attribute('extent_x',str(bb_extent.x)) + # friction_bp.set_attribute('extent_y',str(bb_extent.y)) + # friction_bp.set_attribute('extent_z',str(bb_extent.z)) + # self.sim_world.spawn_actor(friction_bp,self.ego_vehicle.get_transform()) + # self.sim_world.debug.draw_box() + + # let the client interact with server + if self.sync: + if self.pygame: + self._init_renderer() + self.world.restart(self.ego_vehicle) + else: + spectator = self.sim_world.get_spectator() + transform = self.ego_vehicle.get_transform() + spectator.set_transform(carla.Transform(transform.location + carla.Location(z=100), + carla.Rotation(pitch=-90))) + self.sim_world.tick() + else: + self.sim_world.wait_for_tick() + + """Attention: + get_location() Returns the actor's location the client recieved during last tick. The method does not call the simulator. + Hence, upon initializing, the world should first tick before calling get_location, or it could cause fatal bug""" + # self.ego_vehicle.get_location() + + # add route planner for ego vehicle + self.local_planner = LocalPlanner(self.ego_vehicle, {'sampling_resolution': self.sampling_resolution, + 'buffer_size': self.buffer_size, + 'vehicle_proximity': self.vehicle_proximity, + 'traffic_light_proximity': self.traffic_light_proximity}) + # self.local_planner.set_global_plan(self.global_planner.get_route( + # self.map.get_waypoint(self.ego_vehicle.get_location()))) + self.current_lane = get_lane_center(self.map, self.ego_vehicle.get_location()).lane_id + self.last_lane = self.current_lane + self.last_target_lane, self.current_target_lane = self.current_lane, self.current_lane + self.last_action, self.current_action = Action.LANE_FOLLOW, Action.LANE_FOLLOW + self.last_light_state = None + + self.wps_info, self.lights_info, self.vehs_info = self.local_planner.run_step() + + self._ego_autopilot(True) + + # Only use RL controller after ego vehicle speed reach speed_threshold + self.speed_state = SpeedState.START + self.control_sigma = {'Steer': random.choice([0]), + 'Throttle_brake': random.choice([0])} + # self.control_sigma = {'Steer': random.choice([0, 0.05,0.1, 0.1, 0.15]), + # 'Throttle_brake': random.choice([0, 0.05, 0.1, 0.1, 0.15])} + self.autopilot_controller = Basic_Lanechanging_Agent(self.ego_vehicle, dt=1.0 / self.fps, + opt_dict={ + 'ignore_traffic_lights': self.ignore_traffic_light, + 'ignore_stop_signs': True, + 'sampling_resolution': self.sampling_resolution, + 'max_steering': self.steer_bound, + 'max_throttle': self.throttle_bound, + 'max_brake': self.brake_bound, + 'buffer_size': self.buffer_size, + 'target_speed': self.speed_limit, + 'ignore_front_vehicle': False, + 'ignore_change_gap': False, + # 'ignore_front_vehicle': random.choice([False,True]), + # 'ignore_change_gap': random.choice([True, True, False]), + 'lanechanging_fps': random.choice([40, 50, 60]), + 'random_lane_change': False}) + # 'random_lane_change':random.choice([False,True,True,True])}) + # self.controller = ConstantVelocityAgent(self.ego_vehicle,target_speed=self.speed_limit) + # self.controller = BasicAgent(self.ego_vehicle, {'target_speed': self.speed_threshold, 'dt': 1 / self.fps, + # 'max_throttle': self.throttle_bound, + # 'max_brake': self.brake_bound}) + + # code for synchronous mode + # camera_bp = self.sim_world.get_blueprint_library().find('sensor.camera.rgb') + # camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + # self.camera = self.sim_world.spawn_actor(camera_bp, camera_transform, attach_to=self.ego_vehicle) + # self.camera.listen(lambda image: self._sensor_callback(image, self.sensor_queue)) + + # speed state switch + if not self.debug: + if self.total_step - self.rl_control_step < self.pre_train_steps: + # During pre-train steps, let rl and pid alternatively take control + if self.RL_switch: + if self.switch_count >= self.SWITCH_THRESHOLD: + self.RL_switch = False + self.switch_count = 0 + else: + self.switch_count += 1 + else: + self.RL_switch = True + self.switch_count += 1 + else: + self.RL_switch = True + else: + self.RL_switch = False + # self.sim_world.debug.draw_point(self.ego_spawn_point.location,size=0.3,life_time=0) + # while (True): + # spawn_point=random.choice(self.spawn_points).location + # if self.map.get_waypoint(spawn_point).lane_id==self.map.get_waypoint(self.ego_spawn_point.location).lane_id: + # break + # self.controller.set_destination(spawn_point) + + # Update timesteps + self.time_step = 0 + self.reset_step += 1 + + # return state information + return self._get_state() + + def step(self, a_index, action): + self.autopilot_controller.set_info({'left_wps': self.wps_info.left_front_wps, + 'center_wps': self.wps_info.center_front_wps, + 'right_wps': self.wps_info.right_front_wps, + 'left_rear_wps': self.wps_info.left_rear_wps, + 'center_rear_wps': self.wps_info.center_rear_wps, + 'right_rear_wps': self.wps_info.right_rear_wps, + 'vehs_info': self.vehs_info}) + self.step_info.clear() + self.lights_info = None + self.control.steer, self.control.throttle, self.control.brake, self.control.gear = 0.0, 0.0, 0.0, 1 + self.wps_info = WaypointWrapper() + self.vehs_info = VehicleWrapper() + """throttle (float):A scalar value to control the vehicle throttle [0.0, 1.0]. Default is 0.0. + steer (float):A scalar value to control the vehicle steering [-1.0, 1.0]. Default is 0.0. + brake (float):A scalar value to control the vehicle brake [0.0, 1.0]. Default is 0.0.""" + if not self.modify_change_steer: + self.control.steer = np.clip(action[0][0], -self.steer_bound, self.steer_bound) + else: + self.control.steer = float(process_steer(a_index, action[0][0])) + if action[0][1] >= 0: + self.control.brake = 0 + self.control.throttle = np.clip(action[0][1], 0, self.throttle_bound) + else: + self.control.throttle = 0 + self.control.brake = np.clip(abs(action[0][1]), 0, self.brake_bound) + print(f"Steer--After Process:{self.control.steer}, After Recovery:{recover_steer(a_index, self.control.steer)}") + # control = carla.VehicleControl(steer=float(steer), throttle=float(throttle), brake=float(brake),hand_brake=False, + # reverse=False,manual_gear_shift=True,gear=1) + + # Only use RL controller after ego vehicle speed reach speed_threshold + # Use DFA to calculate different speed state transition + if not self.debug: + self._speed_switch(a_index) + else: + self._speed_switch(a_index) + # if self.controller.done() and self.loop: + # while (True): + # spawn_point=random.choice(self.spawn_points).location + # if self.map.get_waypoint(spawn_point).lane_id==self.map.get_waypoint(self.ego_spawn_point.location).lane_id: + # break + # self.controller.set_destination(spawn_point) + # control = self.controller.run_step() + # print("debug mode: last_lane, current lane, last target lane, current target lane, last action, current action: ", + # self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, self.last_action.value,self.current_action.value) + # self.control, self.current_target_lane, self.current_action= \ + # self.autopilot_controller.run_step(self.last_lane, self.current_lane,self.current_target_lane, self.last_action,self.modify_change_steer) + # print("debug mode: last_lane, current lane, last target lane, current target lane, last action, current action: ", + # self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, self.last_action.value,self.current_action.value) + if self.sync: + if not self.debug: + if not self.RL_switch: + # Add noise to autopilot controller's control command + # print(f"Basic Agent Control Before Noise:{control}") + if not self.modify_change_steer: + self.control.steer = np.clip(np.random.normal(self.control.steer, self.control_sigma['Steer']), + -self.steer_bound, self.steer_bound) + else: + if self.current_action == Action.LANE_CHANGE_LEFT: + self.control.steer = np.clip( + np.random.normal(self.control.steer, self.control_sigma['Steer']), + -self.steer_bound, 0) + elif self.current_action == Action.LANE_CHANGE_RIGHT: + self.control.steer = np.clip( + np.random.normal(self.control.steer, self.control_sigma['Steer']), + 0, self.steer_bound) + else: + # LANE_FOLLOW and STOP mode + self.control.steer = np.clip( + np.random.normal(self.control.steer, self.control_sigma['Steer']), + -self.steer_bound, self.steer_bound) + if self.control.throttle > 0: + throttle_brake = self.control.throttle + else: + throttle_brake = -self.control.brake + throttle_brake = np.clip(np.random.normal(throttle_brake, self.control_sigma['Throttle_brake']), + -self.brake_bound, self.throttle_bound) + if throttle_brake > 0: + self.control.throttle = throttle_brake + self.control.brake = 0 + else: + self.control.throttle = 0 + self.control.brake = abs(throttle_brake) + if self.is_effective_action(): + con = carla.VehicleControl(throttle=self.control.throttle, steer=self.control.steer, + brake=self.control.brake, hand_brake=False, reverse=self.control.reverse, + manual_gear_shift=self.control.manual_gear_shift, gear=self.control.gear) + self.ego_vehicle.apply_control(con) + else: + if self.is_effective_action(): + con = carla.VehicleControl(throttle=self.control.throttle, steer=self.control.steer, + brake=self.control.brake, hand_brake=False, reverse=self.control.reverse, + manual_gear_shift=self.control.manual_gear_shift, gear=self.control.gear) + self.ego_vehicle.apply_control(con) + + # print(self.map.get_waypoint(self.ego_vehicle.get_location(),False),self.ego_vehicle.get_transform(),sep='\n') + # print(self.sim_world.get_snapshot().timestamp) + if self.pygame: + self._tick() + else: + spectator = self.sim_world.get_spectator() + transform = self.ego_vehicle.get_transform() + spectator.set_transform(carla.Transform(transform.location + carla.Location(z=80), + carla.Rotation(pitch=-90))) + self.sim_world.tick() + # camera_data = self.sensor_queue.get(block=True) + + """Attention: the server's tick function only returns after it ran a fixed_delta_seconds, so the client need not to wait for + the server, the world snapshot of tick returned already include the next state after the uploaded action.""" + # print(self.map.get_waypoint(self.ego_vehicle.get_location(),False),self.ego_vehicle.get_transform(),sep='\n') + # print(self.sim_world.get_snapshot().timestamp) + # print() + cont = self.ego_vehicle.get_control() + self.control.throttle, self.control.brake, self.control.steer = cont.throttle, cont.brake, cont.steer + self.control.gear, self.control.manual_gear_shift = cont.gear, cont.manual_gear_shift + lane_center = get_lane_center(self.map, self.ego_vehicle.get_location()) + self.current_lane = lane_center.lane_id + # print(self.ego_vehicle.get_speed_limit(),get_speed(self.ego_vehicle,False),get_acceleration(self.ego_vehicle,False),sep='\t') + # route planner + self.wps_info, self.lights_info, self.vehs_info = self.local_planner.run_step() + if self.last_light_state == carla.TrafficLightState.Red and self.lights_info and self.last_light_state != self.lights_info.state: + # light state change during steps, from red to green + self.vel_buffer.clear() + # marks=lane_center.get_landmarks(self.traffic_light_proximity) + # if marks: + # for mark in marks: + # print(f"Mark Road ID:{mark.road_id}, distance:{mark.distance}, name:{mark.distance}") + print( + "After Tick: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + print("Actual Control, change: ", cont, self.current_action.value) + + if self.debug: + # draw_waypoints(self.sim_world, [self.next_wps[0]], 60, z=1) + draw_waypoints(self.sim_world, self.wps_info.center_front_wps + self.wps_info.center_rear_wps + \ + self.wps_info.left_front_wps + self.wps_info.left_rear_wps + self.wps_info.right_front_wps + self.wps_info.right_rear_wps, + 1.0 / self.fps + 0.001, z=1) + else: + # draw_waypoints(self.sim_world, self.wps_info.center_front_wps+self.wps_info.center_rear_wps+\ + # self.wps_info.left_front_wps+self.wps_info.left_rear_wps+self.wps_info.right_front_wps+self.wps_info.right_rear_wps, + # 1.0 / self.fps + 0.001, z=1) + pass + + temp = [] + if self.vehs_info.left_rear_veh is not None: + temp.append(get_speed(self.vehs_info.left_rear_veh, False)) + else: + temp.append(-1) + if self.vehs_info.center_rear_veh is not None: + temp.append(get_speed(self.vehs_info.center_rear_veh, False)) + else: + temp.append(-1) + if self.vehs_info.right_rear_veh is not None: + temp.append(get_speed(self.vehs_info.right_rear_veh, False)) + else: + temp.append(-1) + self.rear_vel_deque.append(temp) + + """Attention: The sequence of following code is pivotal, do not recklessly change their execution order""" + state = self._get_state() + reward = self._get_reward() + truncated = self._truncated() + done = self._done(truncated) + self.step_info.update({'Reward': reward}) + + # update last step info + yaw_forward = lane_center.transform.get_forward_vector().make_unit_vector() + a_3d = self.ego_vehicle.get_acceleration() + self.last_acc, a_t = get_projection(a_3d, yaw_forward) + self.last_yaw = self.ego_vehicle.get_transform().get_forward_vector() + self.last_action = self.current_action + self.last_lane = self.current_lane + self.last_target_lane = self.current_target_lane + if self.lights_info: + self.last_light_state = self.lights_info.state + else: + self.last_light_state = None + else: + temp = self.sim_world.wait_for_tick() + self.sim_world.on_tick(lambda _: {}) + time.sleep(1.0 / self.fps) + reward, state, truncated, done, control_info = None, None, Truncated.FALSE, None, None + + if self.debug: + self.time_step += 1 + self.RL_switch = False + print( + f"Speed:{get_speed(self.ego_vehicle, False)}, Acc:{get_acceleration(self.ego_vehicle, False)}, Time_step:{self.time_step}") + return state, reward, truncated != Truncated.FALSE, done, self._get_info() + + print(f"Current State:{self.speed_state}, RL In Control:{self.RL_switch}") + if not self.RL_switch: + print( + f"Control Sigma -- Steer:{self.control_sigma['Steer']}, Throttle_brake:{self.control_sigma['Throttle_brake']}") + if self.is_effective_action(): + # update timesteps + self.time_step += 1 + self.total_step += 1 + self.vel_buffer.append(self.step_info['velocity']) + if self.RL_switch == True: + self.rl_control_step += 1 + # new_action \in [-1, 0, 1], but saved action is the index of max Q(s, a), and thus change \in [0, 1, 2] + control_info = {'Steer': self.control.steer, 'Throttle': self.control.throttle, 'Brake': self.control.brake, + 'Change': self.current_action, 'control_state': self.RL_switch} + + l_c = self.map.get_waypoint(self.ego_vehicle.get_location()) + print( + f"Episode:{self.reset_step}, Total_step:{self.total_step}, Time_step:{self.time_step}, RL_control_step:{self.rl_control_step}\n" + f"Vel: {self.step_info['velocity']}, Current Acc:{self.step_info['cur_acc']}, Last Acc:{self.step_info['last_acc']}\n" + f"Light State: {self.lights_info.state if self.lights_info else None}, Light Distance:{state['light'][2] * self.traffic_light_proximity}, " + f"Cur Road ID: {lane_center.road_id}, Cur Lane ID: {lane_center.lane_id}, Before Process Road ID: {l_c.road_id}, Lane ID: {l_c.lane_id}\n" + f"Steer:{control_info['Steer']}, Throttle:{control_info['Throttle']}, Brake:{control_info['Brake']}\n" + f"Reward:{self.step_info['Reward']}, Speed Limit:{self.ego_vehicle.get_speed_limit() * 3.6}, Abandon:{self.step_info['Abandon']}") + if truncated == Truncated.FALSE: + print( + f"TTC:{self.step_info['fTTC']}, Comfort:{self.step_info['Comfort']}, Efficiency:{self.step_info['Efficiency']}, " + f"Impact: {self.step_info['impact']}, Change_in_lane_follow:{self.step_info['change_in_lane_follow']}, \n" + f"Off-Lane:{self.step_info['offlane']}, fLcen:{self.step_info['Lane_center']}, " + f"Yaw_change:{self.step_info['yaw_change']}, Yaw_diff:{self.step_info['yaw_diff']}, fYaw:{self.step_info['Yaw']}") + # print(f"Steer:{control_info['Steer']}, Throttle:{control_info['Throttle']}, Brake:{control_info['Brake']}\n") + + return state, reward, truncated != Truncated.FALSE, done, self._get_info(control_info) + else: + return state, reward, truncated != Truncated.FALSE, done, self._get_info() + + def get_observation_space(self): + """ + :return: + """ + """Get observation space of cureent environment""" + return {'waypoints': 10, 'ego_vehicle': 6, 'companion_vehicle': 3, 'light': 3} + + def get_action_bound(self): + """Return action bound of ego vehicle controller""" + return {'steer': self.steer_bound, 'throttle': self.throttle_bound, 'brake': self.brake_bound} + + def is_effective_action(self): + # testing if current ego vehcle's action should be put into replay buffer + return self.speed_state == SpeedState.RUNNING + + def seed(self, seed=None): + return + + def render(self, mode): + pass + + def _get_state(self): + """return a tuple: the first element is next waypoints, the second element is vehicle_front information""" + + left_wps = self.wps_info.left_front_wps + center_wps = self.wps_info.center_front_wps + right_wps = self.wps_info.right_front_wps + + lane_center = get_lane_center(self.map, self.ego_vehicle.get_location()) + right_lane_dis = lane_center.get_right_lane().transform.location.distance(self.ego_vehicle.get_location()) + ego_t = lane_center.lane_width / 2 + lane_center.get_right_lane().lane_width / 2 - right_lane_dis + + ego_vehicle_z = lane_center.transform.location.z + ego_forward_vector = self.ego_vehicle.get_transform().get_forward_vector() + my_sample_ratio = self.buffer_size // 10 + center_wps_processed = process_lane_wp(center_wps, ego_vehicle_z, ego_forward_vector, my_sample_ratio, 0) + if len(left_wps) == 0: + left_wps_processed = center_wps_processed.copy() + for left_wp in left_wps_processed: + left_wp[2] = -1 + else: + left_wps_processed = process_lane_wp(left_wps, ego_vehicle_z, ego_forward_vector, my_sample_ratio, -1) + if len(right_wps) == 0: + right_wps_processed = center_wps_processed.copy() + for right_wp in right_wps_processed: + right_wp[2] = 1 + else: + right_wps_processed = process_lane_wp(right_wps, ego_vehicle_z, ego_forward_vector, my_sample_ratio, 1) + + left_wall = False + if len(left_wps) == 0: + left_wall = True + right_wall = False + if len(right_wps) == 0: + right_wall = True + vehicle_inlane_processed = process_veh(self.ego_vehicle, self.vehs_info, left_wall, right_wall, + self.vehicle_proximity) + + yaw_diff_ego = math.degrees(get_yaw_diff(lane_center.transform.get_forward_vector(), + self.ego_vehicle.get_transform().get_forward_vector())) + + yaw_forward = lane_center.transform.get_forward_vector() + v_3d = self.ego_vehicle.get_velocity() + v_s, v_t = get_projection(v_3d, yaw_forward) + + a_3d = self.ego_vehicle.get_acceleration() + a_s, a_t = get_projection(a_3d, yaw_forward) + + if self.lights_info: + wps = self.lights_info.get_stop_waypoints() + stop_dis = 1.0 + for wp in wps: + if wp.road_id == lane_center.road_id and wp.lane_id == lane_center.lane_id: + stop_dis = wp.transform.location.distance( + lane_center.transform.location) / self.traffic_light_proximity + break + if ( + self.lights_info.state == carla.TrafficLightState.Red or self.lights_info.state == carla.TrafficLightState.Yellow): + light = [0, 1, stop_dis] + else: + light = [1, 0, stop_dis] + else: + stop_dis = 1.0 + light = [1, 0, stop_dis] + + """Attention: + Upon initializing, there are some bugs in the theta_v and theta_a, which could be greater than 90, + this might be caused by carla.""" + self.step_info.update({'velocity': v_s, 'last_acc': self.last_acc, 'cur_acc': a_s}) + # update informatino for rear vehicle + if self.vehs_info.center_rear_veh is None or \ + (self.lights_info is not None and self.lights_info.state != carla.TrafficLightState.Green): + self.step_info.update({'rear_id': -1, 'rear_v': 0, 'rear_a': 0, 'time_step': self.time_step + 1, + 'change_lane': self.current_lane != self.last_lane}) + else: + lane_center = get_lane_center(self.map, self.vehs_info.center_rear_veh.get_location()) + yaw_forward = lane_center.transform.get_forward_vector() + v_3d = self.vehs_info.center_rear_veh.get_velocity() + v_s, v_t = get_projection(v_3d, yaw_forward) + a_3d = self.vehs_info.center_rear_veh.get_acceleration() + a_s, a_t = get_projection(a_3d, yaw_forward) + self.step_info.update({'rear_id': self.vehs_info.center_rear_veh.id, + 'rear_v': v_s, 'rear_a': a_s, 'time_step': self.time_step + 1, + 'change_lane': self.current_lane != self.last_lane}) + + return {'left_waypoints': left_wps_processed, 'center_waypoints': center_wps_processed, + 'right_waypoints': right_wps_processed, 'vehicle_info': vehicle_inlane_processed, + 'ego_vehicle': [v_s / 10, v_t / 10, a_s / 3, a_t / 3, ego_t, yaw_diff_ego / 90], + 'light': light} + + def _get_reward(self): + """Calculate the step reward: + TTC: Time to collide with front vehicle + Eff: Ego vehicle efficiency, speed ralated + Com: Ego vehicle comfort, ego vehicle acceration change rate + Lcen: Distance between ego vehicle location and lane center + """ + truncated = self._truncated() + self.step_info['Abandon'] = False + if truncated != Truncated.FALSE: + if truncated == Truncated.CHANGE_LANE_IN_LANE_FOLLOW: + return -self.lane_penalty + elif truncated == Truncated.COLLISION: + history, tags = self.collision_sensor.get_collision_history() + if SemanticTags.Car in tags or SemanticTags.Truck in tags or SemanticTags.Bus in tags or SemanticTags.Motorcycle in tags \ + or SemanticTags.Rider in tags or SemanticTags.Bicycle in tags: + return -self.penalty + else: + # Abandon the experience that ego vehicle collide with other obstacle + self.step_info['Abandon'] = True + else: + return -self.penalty + + ttc, fTTC = ttc_reward(self.ego_vehicle, self.vehs_info.center_front_veh, self.min_distance, self.TTC_THRESHOLD) + + lane_center = get_lane_center(self.map, self.ego_vehicle.get_location()) + yaw_forward = lane_center.transform.get_forward_vector().make_unit_vector() + + v_3d = self.ego_vehicle.get_velocity() + v_s, v_t = get_projection(v_3d, yaw_forward) + speed_1, speed_2 = self.speed_limit, self.speed_limit + # if self.lights_info and self.lights_info.state!=carla.TrafficLightState.Green: + # wps=self.lights_info.get_stop_waypoints() + # for wp in wps: + # if wp.lane_id==lane_center.lane_id: + # dis=self.ego_vehicle.get_location().distance(wp.transform.location) + # if dis max_speed: + # fEff = 1 + fEff = math.exp(max_speed - v_s * 3.6) + else: + fEff = v_s * 3.6 / max_speed + # if max_speed center_front_dis: + # reward = min((right_front_dis / center_front_dis - 1) * self.lane_change_reward, self.lane_change_reward) + # else: + # reward = max((right_front_dis / center_front_dis - 1) * self.lane_change_reward, -self.lane_change_reward) + # reward = 0 + ttc, rear_ttc_reward = ttc_reward(self.vehs_info.center_rear_veh, self.ego_vehicle, self.min_distance, + self.TTC_THRESHOLD) + # add rear_ttc_reward? + print('lane change reward and rear ttc reward: ', reward, rear_ttc_reward) + elif current_lane - last_lane == 1: + # change left + self.calculate_impact = -1 + center_front_dis = distance_to_front_vehicles[2] + left_front_dis = distance_to_front_vehicles[1] + dis = left_front_dis - center_front_dis + reward = dis / self.vehicle_proximity * self.lane_change_reward + # if left_front_dis > center_front_dis: + # reward = min((left_front_dis / center_front_dis - 1) * self.lane_change_reward, self.lane_change_reward) + # else: + # reward = max((left_front_dis / center_front_dis - 1) * self.lane_change_reward, -self.lane_change_reward) + # reward = 0 + ttc, rear_ttc_reward = ttc_reward(self.vehs_info.center_rear_veh, self.ego_vehicle, self.min_distance, + self.TTC_THRESHOLD) + print('lane change reward and rear ttc reward: ', reward, rear_ttc_reward) + + return reward + + def _truncated(self): + """Calculate whether to terminate the current episode""" + lane_center = get_lane_center(self.map, self.ego_vehicle.get_location()) + yaw_diff = math.degrees(get_yaw_diff(lane_center.transform.get_forward_vector(), + self.ego_vehicle.get_transform().get_forward_vector())) + + if len(self.collision_sensor.get_collision_history()[0]) != 0: + # Here we judge speed state because there might be collision event when spawning vehicles + logging.warn('collison happend') + return Truncated.COLLISION + if not test_waypoint(lane_center, False): + logging.warn('vehicle drive out of road') + return Truncated.OUT_OF_ROAD + if self.current_action == Action.LANE_FOLLOW and self.current_lane != self.last_lane: + logging.warn('change lane in lane following mode') + return Truncated.CHANGE_LANE_IN_LANE_FOLLOW + if self.current_action == Action.LANE_CHANGE_LEFT and self.current_lane - self.last_lane < 0: + logging.warn('vehicle change to wrong lane') + return Truncated.CHANGE_TO_WRONG_LANE + if self.current_action == Action.LANE_CHANGE_RIGHT and self.current_lane - self.last_lane > 0: + logging.warn('vehicle change to wrong lane') + return Truncated.CHANGE_TO_WRONG_LANE + if self.speed_state != SpeedState.START and not self.vehs_info.center_front_veh: + if not self.lights_info or self.lights_info.state != carla.TrafficLightState.Red: + if len(self.vel_buffer) == self.vel_buffer.maxlen: + avg_vel = 0 + for vel in self.vel_buffer: + avg_vel += vel / self.vel_buffer.maxlen + if avg_vel * 3.6 < self.speed_min: + logging.warn('vehicle speed too low') + return Truncated.SPEED_LOW + + # if self.lane_invasion_sensor.get_invasion_count()!=0: + # logging.warn('lane invasion occur') + # return True + # if self.step_info['Lane_center'] <=-1.0: + # logging.warn('drive out of road, lane invasion occur') + # return True + if abs(yaw_diff) > 90: + logging.warn('moving in opposite direction') + return Truncated.OPPOSITE_DIRECTION + if self.lights_info and self.lights_info.state != carla.TrafficLightState.Green: + self.sim_world.debug.draw_point(self.lights_info.get_location(), size=0.3, life_time=0) + wps = self.lights_info.get_stop_waypoints() + for wp in wps: + self.sim_world.debug.draw_point(wp.transform.location, size=0.1, life_time=0) + if is_within_distance_ahead(self.ego_vehicle.get_location(), wp.transform.location, wp.transform, + self.min_distance): + logging.warn('break traffic light rule') + return Truncated.TRAFFIC_LIGHT_BREAK + + return Truncated.FALSE + + def _done(self, truncated): + if truncated != Truncated.FALSE: + return False + if self.wps_info.center_front_wps[2].transform.location.distance( + self.ego_spawn_point.location) < self.sampling_resolution: + # The local planner's waypoint list has been depleted + logging.info('vehicle reach destination, simulation terminate') + return True + if self.wps_info.left_front_wps and \ + self.wps_info.left_front_wps[2].transform.location.distance( + self.ego_spawn_point.location) < self.sampling_resolution: + # The local planner's waypoint list has been depleted + logging.info('vehicle reach destination, simulation terminate') + return True + if self.wps_info.right_front_wps and \ + self.wps_info.right_front_wps[2].transform.location.distance( + self.ego_spawn_point.location) < self.sampling_resolution: + # The local planner's waypoint list has been depleted + logging.info('vehicle reach destination, simulation terminate') + return True + if not self.RL_switch: + if self.time_step > 5000: + # Let the traffic manager only execute 5000 steps. or it can fill the replay buffer + logging.info('5000 steps passed under traffic manager control') + return True + + return False + + def _speed_switch(self, a_index): + """cont: the control command of RL agent""" + ego_speed = get_speed(self.ego_vehicle) + if self.speed_state == SpeedState.START: + # control = self.controller.run_step({'waypoints':self.next_wps,'vehicle_front':self.vehicle_front}) + if ego_speed >= self.speed_threshold: + self.speed_state = SpeedState.RUNNING + self._ego_autopilot(False) + if not self.RL_switch: + # Under basic lanechange agent control + # self.autopilot_controller.set_destination(random.choice(self.spawn_points).location) + # if self.autopilot_controller.done() and self.loop: + # self.autopilot_controller.set_destination(self.my_set_destination()) + # control = self.autopilot_controller.run_step() + print( + "basic_lanechanging_agent before: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + self.control, self.current_target_lane, self.current_action = \ + self.autopilot_controller.run_step(self.current_lane, self.last_target_lane, self.last_action, + self.modify_change_steer) + print( + "basic_lanechanging_agent after: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + else: + if a_index == 0: + self.current_action = Action.LANE_CHANGE_LEFT + self.current_target_lane = self.current_lane + 1 + elif a_index == 2: + self.current_action = Action.LANE_CHANGE_RIGHT + self.current_target_lane = self.current_lane - 1 + elif a_index == 1: + self.current_action = Action.LANE_FOLLOW + self.current_target_lane = self.current_lane + else: + # a_index=4 + self.current_action = Action.STOP + self.current_target_lane = self.current_lane + print( + "initial: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + elif self.speed_state == SpeedState.RUNNING: + if self.RL_switch: + # under rl control, used to set the self.new_action. + print( + "RL_control before: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + if a_index == 0: + self.current_action = Action.LANE_CHANGE_LEFT + self.current_target_lane = self.current_lane + 1 + elif a_index == 2: + self.current_action = Action.LANE_CHANGE_RIGHT + self.current_target_lane = self.current_lane - 1 + elif a_index == 1: + self.current_action = Action.LANE_FOLLOW + self.current_target_lane = self.current_lane + else: + # a_index=4 + self.current_action = Action.STOP + self.current_target_lane = self.current_lane + # _, _, _, self.distance_to_front_vehicles, self.distance_to_rear_vehicles = \ + # self.autopilot_controller.run_step(self.last_lane, self.last_target_lane, self.last_action, True, a_index, self.modify_change_steer) + print( + "RL_control after: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + if ego_speed < self.speed_min: + # Only add reboot state in the beginning 200 episodes + # self._ego_autopilot(True) + # self.speed_state = SpeedState.REBOOT + pass + else: + # Under basic lane change agent control + # if self.autopilot_controller.done() and self.loop: + # # self.autopilot_controller.set_destination(random.choice(self.spawn_points).location) + # self.autopilot_controller.set_destination(self.my_set_destination()) + # control=self.autopilot_controller.run_step() + print( + "basic_lanechanging_agent before: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + self.control, self.current_target_lane, self.current_action = \ + self.autopilot_controller.run_step(self.current_lane, self.last_target_lane, self.last_action, + self.modify_change_steer) + print( + "basic_lanechanging_agent after: last_lane, current_lane, last_target_lane, current_target_lane, last action, current action: ", + self.last_lane, self.current_lane, self.last_target_lane, self.current_target_lane, + self.last_action.value, self.current_action.value) + else: + logging.error('CODE LOGIC ERROR') + + return + + def _get_info(self, control_info=None): + """Rerurn simulation running information, + param: control_info, the current controller information + """ + if control_info is None: + return self.step_info + else: + self.step_info.update(control_info) + return self.step_info + + def _ego_autopilot(self, setting=True): + # Use traffic manager to control ego vehicle + self.ego_vehicle.set_autopilot(setting, self.tm_port) + if setting: + speed_diff = (30 - self.speed_limit) / 30 * 100 + self.traffic_manager.distance_to_leading_vehicle(self.ego_vehicle, self.min_distance) + if self.ignore_traffic_light: + self.traffic_manager.ignore_lights_percentage(self.ego_vehicle, 100) + self.traffic_manager.ignore_walkers_percentage(self.ego_vehicle, 100) + self.traffic_manager.ignore_signs_percentage(self.ego_vehicle, 100) + self.traffic_manager.ignore_vehicles_percentage(self.ego_vehicle, 0) + self.traffic_manager.vehicle_percentage_speed_difference(self.ego_vehicle, speed_diff) + # if self.auto_lanechange and self.speed_state == SpeedState.RUNNING: + self.traffic_manager.auto_lane_change(self.ego_vehicle, True) + self.traffic_manager.random_left_lanechange_percentage(self.ego_vehicle, 0) + self.traffic_manager.random_right_lanechange_percentage(self.ego_vehicle, 0) + + # self.traffic_manager.set_desired_speed(self.ego_vehicle, 72) + # ego_wp=self.map.get_waypoint(self.ego_vehicle.get_location()) + # self.traffic_manager.set_path(self.ego_vehicle,path) + """set_route(self, actor, path): + Sets a list of route instructions for a vehicle to follow while controlled by the Traffic Manager. + The possible route instructions are 'Left', 'Right', 'Straight'. + The traffic manager only need this instruction when faces with a junction.""" + self.traffic_manager.set_route(self.ego_vehicle, + ['Straight', 'Straight', 'Straight', 'Straight', 'Straight', 'Straight', + 'Straight', 'Straight', 'Straight', 'Straight']) + + def _sensor_callback(self, sensor_data, sensor_queue): + array = np.frombuffer(sensor_data.raw_data, dtype=np.dtype('uint8')) + # image is rgba format + array = np.reshape(array, (sensor_data.height, sensor_data.width, 4)) + array = array[:, :, :3] + sensor_queue.put((sensor_data.frame, array)) + + def _tick(self): + self.clock.tick() + # self.sim_world.tick() + if self.sync: + self.world.world.tick() + else: + self.world.world.wait_for_tick() + self.world.tick(self.clock) + self.world.render(self.display) + pygame.display.flip() + + def _init_renderer(self): + """Initialize the birdeye view renderer.""" + pygame.init() + pygame.font.init() + self.display = pygame.display.set_mode( + (self.width, self.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + self.hud = HUD(self.width, self.height) + self.world = World(self.sim_world, self.hud) + self.clock = pygame.time.Clock() + + def _set_synchronous_mode(self): + """Set whether to use the synchronous mode.""" + # Set fixed simulation step for synchronous mode + if self.sync: + settings = self.sim_world.get_settings() + settings.no_rendering_mode = self.no_rendering + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1.0 / self.fps + self.sim_world.apply_settings(settings) + + def _set_traffic_manager(self): + self.traffic_manager = self.client.get_trafficmanager(self.tm_port) + # every vehicle keeps a distance of 3.0 meter + self.traffic_manager.set_global_distance_to_leading_vehicle(10) + # Set physical mode only for cars around ego vehicle to save computation + if self.hybrid: + self.traffic_manager.set_hybrid_physics_mode(True) + self.traffic_manager.set_hybrid_physics_radius(200.0) + + """The default global speed limit is 30 m/s + Vehicles' target speed is 70% of their current speed limit unless any other value is set.""" + speed_diff = (30 * 3.6 - (self.speed_limit + 1)) / (30 * 3.6) * 100 + # Let the companion vehicles drive a bit faster than ego speed limit + self.traffic_manager.global_percentage_speed_difference(-100) + self.traffic_manager.set_synchronous_mode(self.sync) + # set traffic light elpse time + lights_list = self.sim_world.get_actors().filter("*traffic_light*") + for light in lights_list: + light.set_green_time(15) + light.set_red_time(0) + light.set_yellow_time(0) + + def _try_spawn_ego_vehicle_at(self, transform): + """Try to spawn a vehicle at specific transform + Args: + transform: the carla transform object. + + Returns: + Bool indicating whether the spawn is successful. + """ + vehicle = None + # Check if ego position overlaps with surrounding vehicles + overlap = False + for idx, poly in self.vehicle_polygons[-1].items(): + poly_center = np.mean(poly, axis=0) + ego_center = np.array([transform.location.x, transform.location.y]) + dis = np.linalg.norm(poly_center - ego_center) + if dis > 8: + continue + else: + overlap = True + break + + if not overlap: + ego_bp = create_vehicle_blueprint(self.sim_world, self.ego_filter, ego=True, + color=random.choice(['255,0,0', '0,255,0', '0,0,255'])) + vehicle = self.sim_world.try_spawn_actor(ego_bp, transform) + if vehicle is None: + logging.warn("Ego vehicle generation fail") + + # if self.debug and vehicle: + # vehicle.show_debug_telemetry() + + return vehicle + + def _spawn_companion_vehicles(self): + """ + Spawn surrounding vehcles of this simulation + each vehicle is set to autopilot mode and controled by Traffic Maneger + note: the ego vehicle trafficmanager and companion vehicle trafficmanager shouldn't be the same one + """ + # spawn_points_ = self.map.get_spawn_points() + spawn_points_ = self.spawn_points + # make sure companion vehicles also spawn on chosen route + # spawn_points_=[x.transform for x in self.ego_spawn_waypoints] + + num_of_spawn_points = len(spawn_points_) + num_of_vehicles = random.choice(self.num_of_vehicles) + + if num_of_vehicles < num_of_spawn_points: + random.shuffle(spawn_points_) + spawn_points = random.sample(spawn_points_, num_of_vehicles) + else: + msg = 'requested %d vehicles, but could only find %d spawn points' + logging.warning(msg, num_of_vehicles, num_of_spawn_points) + num_of_vehicles = num_of_spawn_points - 1 + + # Use command to apply actions on batch of data + SpawnActor = carla.command.SpawnActor + SetAutopilot = carla.command.SetAutopilot + FutureActor = carla.command.FutureActor # FutureActor is eaqual to 0 + command_batch = [] + + for i, transform in enumerate(spawn_points_): + if i >= num_of_vehicles: + break + + blueprint = create_vehicle_blueprint(self.sim_world, 'vehicle.audi.etron', ego=False, color='0,0,0', + number_of_wheels=[4]) + # Spawn the cars and their autopilot all together + command_batch.append(SpawnActor(blueprint, transform). + then(SetAutopilot(FutureActor, True, self.tm_port))) + + # execute the command batch + for (i, response) in enumerate(self.client.apply_batch_sync(command_batch, self.sync)): + if response.has_error(): + logging.warn(response.error) + else: + # print("Future Actor",response.actor_id) + vehicle = self.sim_world.get_actor(response.actor_id) + self.companion_vehicles.append(vehicle) + + if self.ignore_traffic_light: + self.traffic_manager.ignore_lights_percentage(vehicle, 100) + self.traffic_manager.ignore_walkers_percentage(vehicle, 100) + else: + self.traffic_manager.ignore_lights_percentage(vehicle, 50) + self.traffic_manager.ignore_walkers_percentage(vehicle, 50) + self.traffic_manager.ignore_signs_percentage(vehicle, 100) + self.traffic_manager.auto_lane_change(vehicle, False) + # modify change probability + self.traffic_manager.random_left_lanechange_percentage(vehicle, 0) + self.traffic_manager.random_right_lanechange_percentage(vehicle, 0) + self.traffic_manager.vehicle_percentage_speed_difference(vehicle, + random.choice( + [-100, -100, -100, -140, -160, -180])) + self.traffic_manager.set_route(vehicle, + ['Straight', 'Straight', 'Straight', 'Straight', 'Straight', 'Straight', + 'Straight', 'Straight', 'Straight', 'Straight']) + self.traffic_manager.update_vehicle_lights(vehicle, True) + # print(vehicle.attributes) + + msg = 'requested %d vehicles, generate %d vehicles, press Ctrl+C to exit.' + logging.info(msg, num_of_vehicles, len(self.companion_vehicles)) + + def _try_spawn_random_walker_at(self, transform): + """Try to spawn a walker at specific transform with random bluprint. + + Args: + transform: the carla transform object. + + Returns: + Bool indicating whether the spawn is successful. + """ + pass + + def _clear_actors(self, actor_filters, filter=True): + """Clear specific actors + filter: True means filter actors by blueprint, Fals means fiter actors by carla.CityObjectLabel""" + if filter: + if self.camera is not None: + self.camera.stop() + if self.collision_sensor is not None: + self.collision_sensor.sensor.stop() + if self.lane_invasion_sensor is not None: + self.lane_invasion_sensor.sensor.stop() + for actor_filter in actor_filters: + self.client.apply_batch([carla.command.DestroyActor(x) + for x in self.sim_world.get_actors().filter(actor_filter)]) + + # for actor_filter in actor_filters: + # for actor in self.sim_world.get_actors().filter(actor_filter): + # if actor.is_alive: + # if actor.type_id =='controller.ai.walker': + # actor.stop() + # actor.destroy() diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/settings.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/settings.py new file mode 100644 index 0000000000..77db1b47f2 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/settings.py @@ -0,0 +1,249 @@ +"""This file defines all high level parameters of carla gym environment""" +import argparse +# +# the following road id sets define the chosen route +ROADS = set() +DISTURB_ROADS = set() +FORWARD_ROADS = set() +BACKWARD_ROADS = set() +STRAIGHT = {12, 35, 36} +CURVE = {37, 38, 34} +JUNCTION = {2344, 2035} +DOUBLE_DIRECTION = {2358, 2363, 2039, 2052} +FORWARD_DOUBLE_DIRECTION = {2358, 2039} +BACKWARD_DOUBLE_DIRECTION = {2363, 2052} +# JUNCTION_LANE={33,85,141} +# JUNCTION_LANE_MINUS={102,109,150,163,46,67,128} +ROADS.update(STRAIGHT) +ROADS.update(CURVE) +ROADS.update(JUNCTION) +DISTURB_ROADS.update(DOUBLE_DIRECTION) +FORWARD_ROADS.update(FORWARD_DOUBLE_DIRECTION) +BACKWARD_ROADS.update(BACKWARD_DOUBLE_DIRECTION) +# the flowing arguments set the simulation parameters +ARGS = argparse.ArgumentParser( + description='CARLA_gym Client') +ARGS.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + default=False, + help='Print debug information') +ARGS.add_argument( + '-t', + action='store_true', + dest='train', + default=True, + help='Training Reinforcement agent') +ARGS.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') +ARGS.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') +ARGS.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='Window resolution (default: 1280x720)') +ARGS.add_argument( + '--sync', + action='store_true', + default=True, + help='Synchronous mode execution') +ARGS.add_argument( + '--fps', metavar='FPS', + default=10, type=int, + help="The fps of server running speed") +ARGS.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.tesla.model3', + help='Actor filter (default: "vehicle.*")') +ARGS.add_argument( + '-l', '--loop', + action='store_true', + dest='loop', + default='True', + help='Sets a new random destination upon reaching the previous one (default: False)') +ARGS.add_argument( + "-a", "--agent", type=str, + choices=["Behavior", "Basic"], + help="select which agent to run", + default="Behavior") +ARGS.add_argument( + '-b', '--behavior', type=str, + choices=["cautious", "normal", "aggressive"], + help='Choose one of the possible agent behaviors (default: normal) ', + default='normal') +ARGS.add_argument( + '-s', '--seed', + help='Set seed for repeating executions (default: None)', + default=None, + type=int) +ARGS.add_argument( + '-m', '--map', type=str, + choices=['Town05', 'Town05_Opt'], + help='Choose one of the possible world maps', + default='Town05_Opt') +ARGS.add_argument( + '-n', '--num_of_vehicles', type=list, + help='Total vehicles number which run in simulation', + default=[15*3]) +ARGS.add_argument( + '-sa', '--sampling_resolution', type=float, + help='Distance between generated two waypoints', + default=1.0) +# ARGS.add_argument( +# '-we', '--weather', type=float, +# help='weather setting', +# default=sunlight) +ARGS.add_argument( + '--tm-port', + metavar='P', + default=8000, + type=int, + help='Port to communicate with traffic manager (default: 8000)') +ARGS.add_argument( + '--hybrid', + action='store_true', + default=True, + help='Activate hybrid mode for Traffic Manager') +ARGS.add_argument( + '--auto_lane_change', + action='store_true', + default=True, + help='set lane change behaviors for Traffic Manager') +ARGS.add_argument( + '--no_rendering', + action='store_true', + default=False, + help='Activate no rendering mode') +ARGS.add_argument( + '--stride', type=int, + default=10, + help='The number of waypoints observed by the autonomous vehicle') +ARGS.add_argument( + '--traffic_light_th', type=float, + default=50, + help='the detection range of traffic light') +ARGS.add_argument( + '--vehicle_th', type=float, + default=70, + help='the detection range of conventional vehicles') +ARGS.add_argument( + '--TTC_th', type=float, + default=4, + help='TTC threshold') +ARGS.add_argument( + '--acceleration threshold', type=float, + default=3, + help='acceleration threshold for ego vehicle') +ARGS.add_argument( + '--speed threshold', type=float, + default=0.1, + help='speed threshold for ego vehicle') +# ARGS.add_argument( +# '--traffic_light_th', type=float, +# default=50, +# help='the detection range of traffic light') +# ARGS.add_argument( +# '--vehicle_th', type=float, +# default=70, +# help='the detection range of conventional vehicles') +ARGS.add_argument( + '--penalty', type=float, + default=40, + help='reward penalty for simulation terminated early on account of collision and road invasion') +ARGS.add_argument( + '--lane_penalty', type=float, + default=20, + help='reward penalty for simulation terminated early on account of lane invasion') +ARGS.add_argument( + '--lane_change_reward', type=float, + default=25, + help='reward for lane change according to the distance to the preceding vehicle') +# ARGS.add_argument( +# '--acceleration threshold', type=float, +# default=3, +# help='acceleration threshold for ego vehicle') +# ARGS.add_argument( +# '--speed threshold', type=float, +# default=0.1, +# help='speed threshold for ego vehicle') +ARGS.add_argument( + '--speed_limit', type=float, + default=90.0, + help='Speed limit for ego vehicle, km/h') +ARGS.add_argument( + '--speed_threshold', type=float, + default=20.0, + help='Speed threshold for ego vehicle, start phase for ego vehicle, km/h') +ARGS.add_argument( + '--speed_min', type=float, + default=3.6, + help='When ego vehicle speed reaches down to this threshold, we should let basic agent take control \ + and the action of basic need to add into the replay buffer, km/h') +ARGS.add_argument( + '--steer_bound', type=float, + default=1.0, + help='Steer bound for ego vehicle controller') +ARGS.add_argument( + '--throttle_bound', type=float, + default=1.0, + help='Throttle bound for ego vehicle controller') +ARGS.add_argument( + '--brake_bound', type=float, + default=1.0, + help='Brake bound for ego vehicle controller') +ARGS.add_argument( + '--pre_train_steps', type=int, + default=320000, + help='During pre-train steps, agent is only under PID control.') +ARGS.add_argument( + '--switch_threshold', type=int, + default=30, + help='Let the RL controller and PID controller alternatively take control every 20 episodes') +ARGS.add_argument( + '--vehicle_proximity', type=float, + default=50.0, + help='Distance for searching vehicles in front of ego vehicle, unit -- meters') +ARGS.add_argument( + '--traffic_light_proximity', type=float, + default=50.0, + help='Distance for searching traffic light in front of ego vehicle, unit -- meters,' + 'attention: this value is tricky') +ARGS.add_argument( + '--min_distance',type=float, + default=5.0, + help='Min distance between two vehicles, unit -- meters') +ARGS.add_argument( + '--guide_change', type=bool, + default=False, + help='guide the vehicle to change via lane_center') +ARGS.add_argument( + '--ignore_traffic_light', type=bool, + default=False, + help='Set the vehicles in simulation to ignore traffic lights or not') +ARGS.add_argument( + '--ego_num', type=int, + default=1, + help='The number of RL controlled ego vehicle') +ARGS.add_argument( + '--modify_change_steer', type=bool, + default=True, + help='Useful for pdqn training, make sure the steer >0 when current action is ACTION.CHANGE_LANE_LEFT, \ + and the steer < 0 when current action is Action.CHANGE_LANE_LEFT') +ARGS.add_argument( + '--pygame', type=bool, + default=False, + help='Render another pygame window for ego vehicle and the window style looks like automatic_control.py') +ARGS.add_argument( + '--alg', type=str, + default='PDQN', + help='The RL algorithm currently in use') \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/__init__.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/bridge_functions.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/bridge_functions.py new file mode 100644 index 0000000000..7c75b926c4 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/bridge_functions.py @@ -0,0 +1,286 @@ +import numpy as np +import cv2 +import sys +import utm +import math + + +def lidar_string_to_array(lidar, half_cloud=None, whole_cloud=None): + """ + Return the LiDAR pointcloud in numpy.array format based on a string. Every time, half (in this case) of the cloud + is computed due to the LiDAR frequency, so if whole_cloud == True, we concatenate two consecutive pointclouds + """ + lidar_data = np.fromstring(lidar, dtype=np.float32) + lidar_data = np.reshape(lidar_data, (int(lidar_data.shape[0] / 4), 4)) + + # We take the oposite of y axis (since in CARLA a LiDAR point is + # expressed in left-handed coordinate system, and ROS needs right-handed) + + lidar_data[:, 1] *= -1 + + if whole_cloud: + lidar_data = np.concatenate((half_cloud, lidar_data), axis=0) + + return lidar_data + + +def cv2_to_imgmsg(cvim, encoding="passthrough"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param encoding: The encoding of the image data, one of the following strings: + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + :rtype: A sensor_msgs.msg.Image message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` + If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type. + Otherwise desired_encoding must be one of the standard image encodings + This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. + """ + + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError('Your input type is not a numpy array') + img_msg = Image() + img_msg.height = cvim.shape[0] + img_msg.width = cvim.shape[1] + + if len(cvim.shape) < 3: + cv_type = 'mono8' + else: + cv_type = 'bgr8' + if encoding == "passthrough": + img_msg.encoding = cv_type + else: + img_msg.encoding = encoding + + if cvim.dtype.byteorder == '>': + img_msg.is_bigendian = True + img_msg.data = cvim.tostring() + img_msg.step = len(img_msg.data) // img_msg.height + + return img_msg + + +def build_camera_info(width, height, f_x, f_y, x, y, current_ros_time, frame_id, distorted_image=None): + """ + Private function to compute camera info + camera info doesn't change over time + """ + camera_info = CameraInfo() + camera_info.header.stamp = current_ros_time + camera_info.header.frame_id = frame_id + camera_info.width = width + camera_info.height = height + camera_info.distortion_model = 'plumb_bob' + cx = camera_info.width / 2.0 + cy = camera_info.height / 2.0 + fx = f_x + fy = f_y + camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] + + if not distorted_image: + camera_info.D = [0, 0, 0, 0, 0] + camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] + camera_info.P = [fx, 0, cx, x, 0, fy, cy, y, 0, 0, 1.0, 0] + + return camera_info + else: + return np.array([camera_info.K]).reshape(3, 3) # Only return intrinsic parameters + + +def build_camera_info_from_file(frame, x_pos, y_pos, current_ros_time, + camera_parameters_path='/workspace/team_code/generic_modules/camera_parameters/'): + """ + Private function to compute camera info + camera info doesn't change over time + """ + + x = x_pos + y = y_pos + + K = np.loadtxt(camera_parameters_path + 'K.txt') + fx = K[0, 0] + fy = K[1, 1] + cx = K[0, 2] + cy = K[1, 2] + + # print("x, y: ", x, y) + # print("fx, fy, cx, cy: ", fx, fy, cx, cy) + + roi = np.loadtxt(camera_parameters_path + 'roi.txt') + xtl, ytl, width, height = roi + width = int(width) + height = int(height) + + camera_info = CameraInfo() + camera_info.header.stamp = current_ros_time + camera_info.header.frame_id = frame + camera_info.width = width + camera_info.height = height + camera_info.distortion_model = 'plumb_bob' + + camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] + camera_info.D = [0, 0, 0, 0, 0] + camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] + camera_info.P = [fx, 0, cx, x, 0, fy, cy, y, 0, 0, 1.0, 0] + + return camera_info + + +def image_rectification(distorted_image, + camera_parameters_path='/workspace/team_code/generic_modules/camera_parameters/'): + """ + """ + + K_distorted = np.loadtxt(camera_parameters_path + 'K_original.txt') # Load your original K matrix + D = np.loadtxt(camera_parameters_path + 'D.txt') # Load the distortion coefficients of your original image + roi = np.loadtxt(camera_parameters_path + 'roi.txt').astype(np.int64) # Load ROI dimensions + + h_dist, w_dist = distorted_image.shape[:2] + + x, y, w, h = roi + + dst = cv2.undistort(distorted_image, K_distorted, D, None) # Undistort + dst = dst[y:y + h, x:x + w] + return dst + + +def get_routeNodes(route): + """ + Returns the route in Node3D format to visualize it on RVIZ + """ + + nodes = [] + + for waypoint in route: + node = monitor_classes.Node3D() + node.x = waypoint.transform.location.x + node.y = -waypoint.transform.location.y + node.z = 0 + nodes.append(node) + return nodes + + +def process_localization(ekf, gnss, imu, actual_speed, current_ros_time, map_frame, base_link_frame, enabled_pose, + count_localization, init_flag, last_yaw): + """ + Return UTM position (x,y,z) and orientation of the ego-vehicle as a nav_msgs.Odometry ROS message based on the + gnss information (WGS84) and imu (to compute the orientation) + GNSS -> latitude = gnss[0] ; longitude = gnss[1] ; altitude = gnss[2] + IMU -> accelerometer.x = imu[0] ; accelerometer.y = imu[1] ; accelerometer.z = imu[2] ; + gyroscope.x = imu[3] ; gyroscope.y = imu[4] ; gyroscope.z = imu[5] ; compass = imu[6] + """ + + # Read GNSS data + latitude = -gnss[0] # Negative y to correspond to carla axis + longitude = gnss[1] + altitude = gnss[2] + + # Convert Geographic (latitude, longitude) to UTM (x,y) coordinates + EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name + scale = math.cos(latitude * math.pi / 180.0) + x = scale * longitude * math.pi * EARTH_RADIUS_EQUA / 180.0 + # Negative y to correspond to carla documentations + y = - scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + latitude) * math.pi / 360.0)) + + # Read IMU data -> Yaw angle is used to give orientation to the gnss pose + roll = 0 + pitch = 0 + compass = imu[6] + yaw_velocity = -imu[5] ##Carla tiene los ejes de coordenadas detodo (mapa, sensores...) con la Y en sentido opuesto + + if np.isnan(compass): # Sometimes we receive NaN measurement by compass + yaw = last_yaw + print("ERROR NaN received by IMU") + else: + if (0 < compass < math.radians(180)): + yaw = -compass + math.radians(90) + else: + yaw = -compass + math.radians(450) + last_yaw = yaw + [qx, qy, qz, qw] = euler_to_quaternion(roll, pitch, yaw) + + # Process EKF filter + if init_flag: + ekf = Localization_EKF(np.array([x, y, yaw])) + x_filtered, y_filtered = ekf.kalman_filter(x, y, actual_speed, yaw_velocity) + + # Filling in ROS messages for publishing + gnss_pose_msg = Odometry() + gnss_pose_msg.header.frame_id = map_frame + gnss_pose_msg.child_frame_id = base_link_frame + gnss_pose_msg.header.stamp = current_ros_time + gnss_pose_msg.pose.pose.position.x = x + gnss_pose_msg.pose.pose.position.y = y + gnss_pose_msg.pose.pose.position.z = 0 + gnss_pose_msg.pose.pose.orientation.x = qx + gnss_pose_msg.pose.pose.orientation.y = qy + gnss_pose_msg.pose.pose.orientation.z = qz + gnss_pose_msg.pose.pose.orientation.w = qw + gnss_pose_msg.twist.twist.linear.x = actual_speed + gnss_pose_msg.twist.twist.angular.z = yaw_velocity + gnss_translation_error = 0.55 # std. deviation -> error_lat = error_long = 0.000005deg -> x_error = y_error = 0.55m + gnss_rotation_error = 0.0 # error compass = 0 deg + gnss_pose_msg.pose.covariance = np.diag( + [gnss_translation_error, gnss_translation_error, 0.0, 0.0, 0.0, gnss_rotation_error]).ravel() + speedometer_error = 0.0 + imu_gyroscope_error = 0.001 # standard deviation of yaw rate in rad/s + gnss_pose_msg.twist.covariance = np.diag([speedometer_error, 0.0, 0.0, 0.0, 0.0, imu_gyroscope_error]).ravel() + + filtered_pose_msg = Odometry() + filtered_pose_msg.header.frame_id = map_frame + filtered_pose_msg.child_frame_id = base_link_frame + filtered_pose_msg.header.stamp = current_ros_time + filtered_pose_msg.pose.pose.position.x = x_filtered + filtered_pose_msg.pose.pose.position.y = y_filtered + filtered_pose_msg.pose.pose.position.z = 0 + filtered_pose_msg.pose.pose.orientation.x = qx + filtered_pose_msg.pose.pose.orientation.y = qy + filtered_pose_msg.pose.pose.orientation.z = qz + filtered_pose_msg.pose.pose.orientation.w = qw + filtered_pose_msg.twist.twist.linear.x = actual_speed + + if not enabled_pose: ##Espera de 1seg converger EKF + gnss_translation_error = 0.01 # [m] + count_localization += 1 + if (count_localization >= 20): + enabled_pose = True + + return ekf, filtered_pose_msg, gnss_pose_msg, enabled_pose, count_localization, last_yaw + + +class Localization_EKF(): + """ + """ + + def __init__(self, initial_obs): + """ + initial_obs: numpy array (x,y,yaw) w.r.t. map + """ + xy_obs_noise_std = 0.556597453966366 # standard deviation of observation noise of x and y in meter + initial_yaw_std = 0.0 # standard deviation of observation noise of yaw in radian + forward_velocity_noise_std = 0.0 + yaw_rate_noise_std = 0.001 # standard deviation of yaw rate in rad/s + + self.P = np.array([ + [xy_obs_noise_std ** 2., 0., 0.], + [0., xy_obs_noise_std ** 2., 0.], + [0., 0., initial_yaw_std ** 2.]]) + self.Q = np.array([ + [xy_obs_noise_std ** 2., 0.], + [0., xy_obs_noise_std ** 2.]]) + self.R = np.array([ + [forward_velocity_noise_std ** 2., 0., 0.], + [0., forward_velocity_noise_std ** 2., 0.], + [0., 0., yaw_rate_noise_std ** 2.]]) + + self.kf = EKF(initial_obs, self.P) + self.dt = 0.05 + + def kalman_filter(self, x, y, lineal_velocity_x, ang_velocity_yaw): + u = np.array([lineal_velocity_x, ang_velocity_yaw]) + self.kf.propagate(u, self.dt, self.R) + z = np.array([x, y]) + self.kf.update(z, self.Q) + return self.kf.x[0], self.kf.x[1] + diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/extended_kalman_filter.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/extended_kalman_filter.py new file mode 100644 index 0000000000..6403a634ee --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/extended_kalman_filter.py @@ -0,0 +1,68 @@ +# references: +# [1] https://docs.ufpr.br/~danielsantos/ProbabilisticRobotics.pdf + +import numpy as np + + +class ExtendedKalmanFilter: + """Extended Kalman Filter + for vehicle whose motion is modeled as eq. (5.9) in [1] + and with observation of its 2d location (x, y) + """ + def __init__(self, x, P): + """ + Args: + x (numpy.array): state to estimate: [x_, y_, theta]^T + P (numpy.array): estimation error covariance + """ + self.x = x # [3,] + self.P = P # [3, 3] + + def update(self, z, Q): + """update x and P based on observation of (x_, y_) + Args: + z (numpy.array): obsrervation for [x_, y_]^T + Q (numpy.array): observation noise covariance + """ + # compute Kalman gain + H = np.array([ + [1., 0., 0.], + [0., 1., 0.] + ]) # Jacobian of observation function + + K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + Q) + + # update state x + x, y, theta = self.x + z_ = np.array([x, y]) # expected observation from the estimated state + self.x = self.x + K @ (z - z_) + + # update covariance P + self.P = self.P - K @ H @ self.P + + def propagate(self, u, dt, R): + """propagate x and P based on state transition model defined as eq. (5.9) in [1] + Args: + u (numpy.array): control input: [v, omega]^T + dt (float): time interval in second + R (numpy.array): state transition noise covariance + """ + # propagate state x + x, y, theta = self.x + v, omega = u + r = v / omega # turning radius + + dtheta = omega * dt + dx = - r * np.sin(theta) + r * np.sin(theta + dtheta) + dy = + r * np.cos(theta) - r * np.cos(theta + dtheta) + + self.x += np.array([dx, dy, dtheta]) + + # propagate covariance P + G = np.array([ + [1., 0., - r * np.cos(theta) + r * np.cos(theta + dtheta)], + [0., 1., - r * np.sin(theta) + r * np.sin(theta + dtheta)], + [0., 0., 1.] + ]) # Jacobian of state transition function + + self.P = G @ self.P @ G.T + R diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/geometric_functions.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/geometric_functions.py new file mode 100644 index 0000000000..86bff20f40 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/geometric_functions.py @@ -0,0 +1,18 @@ +import numpy as np + +def euler_to_quaternion(roll, pitch, yaw): + """ + Return the orientation of our ego-vehicle in quaternion based on Euler angles (Roll = x, Pitch = y, Yaw = z) + """ + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + + return [qx, qy, qz, qw] + +def unit_vector(vector): + """ + Return the corresponding unit vector + """ + return vector / np.linalg.norm(vector) \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/misc.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/misc.py new file mode 100644 index 0000000000..7bc57714a2 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/misc.py @@ -0,0 +1,485 @@ +""" Module with auxiliary functions. All following functions are related with carla environment""" +import logging +import math, re +import carla +import random +import numpy as np +from gym_carla.multi_lane.settings import * +from enum import Enum + + +# +def remove_unnecessary_objects(world): + """Remove unuseful objects in the world""" + + def remove_object(world, objs, obj): + for ob in world.get_environment_objects(obj): + objs.add(ob.id) + + world.unload_map_layer(carla.MapLayer.StreetLights) + world.unload_map_layer(carla.MapLayer.Buildings) + world.unload_map_layer(carla.MapLayer.Decals) + world.unload_map_layer(carla.MapLayer.Walls) + world.unload_map_layer(carla.MapLayer.Foliage) + world.unload_map_layer(carla.MapLayer.ParkedVehicles) + # world.unload_map_layer(carla.MapLayer.Particles) + world.unload_map_layer(carla.MapLayer.Ground) + labels = [carla.CityObjectLabel.TrafficSigns, carla.CityObjectLabel.Other, + carla.CityObjectLabel.Poles, carla.CityObjectLabel.Static, carla.CityObjectLabel.Dynamic, + carla.CityObjectLabel.Buildings, + carla.CityObjectLabel.Fences, carla.CityObjectLabel.Walls, carla.CityObjectLabel.Vegetation, + carla.CityObjectLabel.Ground] + objs = set() + for label in labels: + for obj in world.get_environment_objects(label): + objs.add(obj.id) + world.enable_environment_objects(objs, False) + # world.unload_map_layer(carla.MapLayer.Props) + + +def test_waypoint(waypoint, ego=False): + """ + test if a given waypoint is on chosen route + :param reward: + True means the tested waypoint is on normal road + False means the tested waypoint is in junction + """ + # if (waypoint.road_id in STRAIGHT or waypoint.road_id in JUNCTION) and waypoint.lane_id == -1: + # return True + # if waypoint.road_id in CURVE and waypoint.lane_id == 1: + # return True + if not ego: + if (waypoint.road_id in ROADS or waypoint.road_id in DISTURB_ROADS) and ( + waypoint.lane_id == -1 or waypoint.lane_id == -2 or waypoint.lane_id == -3): + return True + return False + else: + if waypoint.road_id in ROADS and (waypoint.lane_id == -1 or waypoint.lane_id == -2 or waypoint.lane_id == -3): + return True + return False + + +def draw_waypoints(world, waypoints, life_time=0.0, z=0.5): + """ + Draw a list of waypoints at a certain height given in z. + + :param world: carla.world object + :param waypoints: list or iterable container with the waypoints to draw + :param z: height in meters + """ + for wpt in waypoints: + wpt_t = wpt.transform + begin = wpt_t.location + carla.Location(z=z) + angle = math.radians(wpt_t.rotation.yaw) + end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) + world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=life_time) + + +def get_lane_center(map, location): + """Project current loction to its lane center, return lane center waypoint""" + # test code for junction lane invasion bug + # if lane_center.is_junction: + # test=self.map.get_waypoint(self.ego_vehicle.get_location(),project_to_road=True,lane_type=carla.LaneType.Shoulder) + # test_l=test.get_left_lane() + # print(lane_center.road_id,lane_center.lane_id,test.road_id, + # test.lane_id,test_l.road_id,test_l.lane_id, + # lane_center.transform.location,test_l.transform.location,sep='\t') + # lane_center=map.get_waypoint(location,project_to_road=True) + # if lane_center.is_junction: + # """If ego vehicle is in junction, to avoid bug in get_waypoint function, + # first project the ego vehicle location to the road shoulder lane, then get the straight lane waypoint + # according to the relative location between shoulder lane and current driving lane""" + # shoulder = map.get_waypoint(location, project_to_road=True,lane_type=carla.LaneType.Shoulder) + # lane_center = shoulder.get_left_lane() + + lane_center = map.get_waypoint(location, project_to_road=True, + lane_type=carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk) + road_id = lane_center.road_id + lane_id = lane_center.lane_id + # print('before process road_id and lane_id: ', road_id, lane_id) + in_road = road_id in ROADS + + if road_id in DISTURB_ROADS: + # in a left+straight, get_right_lane = None, for example: road 2039 in Town05 + # """ + # if ego vehicle not in the specific roads, we first get the right waypoint of lanecenter + # and then get the left waypoint of the right waypoint + # finally, check lanecenter whether is in lane -1 + # if != -1, it indicate lanecenter in shoulder, thus get the right waypoint + # """ + # print('in_road: ', location, lane_center, in_road, road_id, ROADS) + # right = lane_center.get_right_lane() + # lane_center = right.get_left_lane() + # if lane_center.lane_id != -1: + # lane_center = lane_center.get_right_lane() + lane_shoulder = map.get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Shoulder) + # print('lane_shoulder, lane_shoulder.lane_id, road_id, lane_id: ', lane_shoulder, lane_shoulder.lane_id, road_id, lane_id) + if lane_shoulder.lane_id == 1: + lane_center_right = lane_shoulder.get_right_lane() + lane_center_left = lane_shoulder.get_left_lane() + # if lane_center_right is not None: + # print('lane_center_right', lane_center_right, lane_center_right.lane_id) + # else: + # print('right is None') + # if lane_center_left is not None: + # print('lane_center_left', lane_center_left, lane_center_left.lane_id) + # else: + # print('left is None') + if (lane_center_right is None or lane_center_right.lane_id != -1) and ( + lane_center_left is None or lane_center_left.lane_id != -1): + logging.error('get lane error!!') + lane_Sidewalk = map.get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Sidewalk) + if lane_Sidewalk.lane_id == -5: + # print('lane_shoulder.lane_id == -5') + lane_center = lane_Sidewalk.get_left_lane().get_left_lane().get_left_lane().get_left_lane() + elif lane_Sidewalk.lane_id == -6: + # print('lane_shoulder.lane_id == -6') + lane_center = lane_Sidewalk.get_left_lane().get_left_lane().get_left_lane().get_left_lane().get_left_lane() + elif lane_center_left is not None and lane_center_left.lane_id == -1: + lane_center = lane_center_left + elif lane_center_right is not None and lane_center_right.lane_id == -1: + lane_center = lane_center_right + elif lane_shoulder.lane_id == -5: + # print('lane_shoulder.lane_id == -5') + lane_center = lane_shoulder.get_left_lane().get_left_lane().get_left_lane().get_left_lane() + elif lane_shoulder.lane_id == -6: + # print('lane_shoulder.lane_id == -6') + lane_center = lane_shoulder.get_left_lane().get_left_lane().get_left_lane().get_left_lane().get_left_lane() + # print('lane_center.road_id: ', lane_center.road_id) + return lane_center + + +# def get_yaw_diff(rotation1, rotation2): +# if abs(rotation1.yaw - rotation2.yaw) < 90: +# yaw_diff = rotation1.yaw - rotation2.yaw +# else: +# yaw_diff = (rotation1.yaw + 720) % 360 - (rotation2.yaw + 720) % 360 +# if abs(yaw_diff) < 90: +# yaw_diff /= 90 +# else: +# # The current state is not stable, deprecate it +# yaw_diff = np.sign(yaw_diff) +# +# return yaw_diff +def get_yaw_diff(vector1, vector2): + """ + Get two vectors' yaw difference in radians (0-PI), + The vector format should be carla.Vector3D, and we set the vectors' z value to 0 because of the map been working on. + """ + vector1.z = 0 + vector2.z = 0 + # vector1=vector1.make_unit_vector() + # vector2=vector2.make_unit_vector() + theta_sign = get_sign(vector1, vector2) + if vector1.length() != 0.0 and vector2.length() != 0.0: + theta = math.acos( + np.clip(vector1.dot(vector2) / (vector1.length() * vector2.length()), -1, 1)) + else: + theta = math.acos(0) + return theta_sign * theta + + +def get_projection(vector1, vector2): + """ + Return the projection of vector1 on vector2, + proj_s is horizontal projection, proj_t is vertical projection + vector1 and vector2 should be carla.Vector3D type + """ + # vector1.z=0 + # vector2.z=0 + theta = get_yaw_diff(vector1, vector2) + proj_s = vector1.length() * math.cos(theta) + proj_t = vector1.length() * math.sin(theta) + + return proj_s, proj_t + + +def get_sign(vector1, vector2): + """negative value means vector1 is on the left of vector2, positive is on the right of vector2. + The vector format should be carla.Vector3D, and we set the vectors' z value to 0 because of the map been working on.""" + vector1.z = 0 + vector2.z = 0 + + sign = 1 if vector1.cross(vector2).z >= 0 else -1 + return sign + + +def get_speed(vehicle, unit=True): + """ + Compute speed of a vehicle, ignore z value + :param unit: the unit of return, True means Km/h, False means m/s + :param vehicle: the vehicle for which speed is calculated + :return: speed as a float + """ + vel = vehicle.get_velocity() + + if unit: + return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2) + else: + return math.sqrt(vel.x ** 2 + vel.y ** 2) + + +def get_acceleration(vehicle, unit=True): + """ + Compute acceleration of a vehicle + :param unit: the unit of return, True means Km/h^2, False means m/s^2 + :param vehicle: the vehicle for which speed is calculated + :return: acceleration as a float + """ + acc = vehicle.get_acceleration() + + if unit: + return 36 * 36 * 10 * math.sqrt(acc.x ** 2 + acc.y ** 2) + else: + return math.sqrt(acc.x ** 2 + acc.y ** 2) + + +def get_actor_polygons(world, filt): + """Get the bounding box polygon of actors. + Args: + filt: the filter indicating what type of actors we'll look at. + world: carla.world + Returns: + actor_poly_dict: a dictionary containing the bounding boxes of specific actors. + """ + actor_poly_dict = {} + for actor in world.get_actors().filter(filt): + # Get x, y and yaw of the actor + trans = actor.get_transform() + x = trans.location.x + y = trans.location.y + yaw = trans.rotation.yaw / 180 * np.pi + # Get length and width + bb = actor.bounding_box + l = bb.extent.x + w = bb.extent.y + # Get bounding box polygon in the actor's local coordinate + poly_local = np.array([[l, w], [l, -w], [-l, -w], [-l, w]]).transpose() + # Get rotation matrix to transform to global coordinate + R = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) + # Get global bounding box polygon + poly = np.matmul(R, poly_local).transpose() + np.repeat([[x, y]], 4, axis=0) + actor_poly_dict[actor.id] = poly + return actor_poly_dict + + +def get_trafficlight_trigger_location(traffic_light): + """ + Calculates the yaw of the waypoint that represents the trigger volume of the traffic light + """ + + def rotate_point(point, radians): + """ + rotate a given point by a given angle + """ + rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y + rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y + + return carla.Vector3D(rotated_x, rotated_y, point.z) + + base_transform = traffic_light.get_transform() + base_rot = base_transform.rotation.yaw + area_loc = base_transform.transform(traffic_light.trigger_volume.location) + area_ext = traffic_light.trigger_volume.extent + # print('extent and transform: ', area_ext.x, area_ext.y, area_ext.z, base_transform) + + point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) + point_location = area_loc + carla.Location(x=point.x, y=point.y) + + return carla.Location(point_location.x, point_location.y, point_location.z) + + +def is_within_distance(target_transform, reference_transform, max_distance, angle_interval=None): + """ + Check if a location is both within a certain distance from a reference object. + By using 'angle_interval', the angle between the location and reference transform + will also be taken into account, being 0 a location in front and 180, one behind. + + :param target_transform: location of the target object + :param reference_transform: location of the reference object + :param max_distance: maximum allowed distance + :param angle_interval: only locations between [min, max] angles will be considered. This isn't checked by default. + :return: boolean + """ + target_vector = np.array([ + target_transform.location.x - reference_transform.location.x, + target_transform.location.y - reference_transform.location.y + ]) + norm_target = np.linalg.norm(target_vector) + + # If the vector is too short, we can simply stop here + if norm_target < 0.001: + return True + + # Further than the max distance + if norm_target > max_distance: + return False + + # We don't care about the angle, nothing else to check + if not angle_interval: + return True + + min_angle = angle_interval[0] + max_angle = angle_interval[1] + + fwd = reference_transform.get_forward_vector() + forward_vector = np.array([fwd.x, fwd.y]) + angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return min_angle < angle < max_angle + + +def is_within_distance_ahead(target_location, current_location, current_transform, max_distance): + """ + Check if a target object is within a certain distance in front of a reference object. + + :param target_location: location of the target object + :param current_location: location of the reference object + :param current_transform: transform of the reference object + :param max_distance: maximum allowed distance + :return: True if target object is within max_distance ahead of the reference object + """ + target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + if norm_target < 0.001: + return True + if norm_target > max_distance: + return False + + # forward_vector = np.array( + # [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + fwd = current_transform.get_forward_vector() + forward_vector = np.array([fwd.x, fwd.y]) + d_angle = math.degrees(math.acos( + np.clip(np.dot(forward_vector, target_vector) / norm_target, -1, 1))) + + return 0.0 < d_angle < 90.0 + + +def is_within_distance_rear(target_location, current_location, current_transform, max_distance): + """ + Check if a target object is within a certain distance in front of a reference object. + + :param target_location: location of the target object + :param current_location: location of the reference object + :param current_transform: transform of the reference object + :param max_distance: maximum allowed distance + :return: True if target object is within max_distance ahead of the reference object + """ + target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + if norm_target < 0.001: + return True + if norm_target > max_distance: + return False + + # forward_vector = np.array( + # [math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + fwd = current_transform.get_forward_vector() + forward_vector = np.array([fwd.x, fwd.y]) + d_angle = math.degrees(math.acos( + np.clip(np.dot(forward_vector, target_vector) / norm_target, -1, 1))) + + return 90.0 < d_angle < 180.0 + + +def compute_magnitude_angle(target_location, current_location, orientation): + """ + Compute relative angle and distance between a target_location and a current_location + + :param target_location: location of the target object + :param current_location: location of the reference object + :param orientation: orientation of the reference object + :return: a tuple composed by the distance to the object and the angle between both objects + """ + target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + + forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return (norm_target, d_angle) + + +def vector(location_1, location_2): + """ + Returns the unit vector from location_1 to location_2 + + :param location_1, location_2: carla.Location objects + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps + + return [x / norm, y / norm, z / norm] + + +def compute_distance(location_1, location_2): + """ + Euclidean distance between 3D points + This map's z value does not change, so ignore the location's z value + :param location_1, location_2: 3D points + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps + return norm + + +def positive(num): + """ + Return the given number if positive, else 0 + + :param num: value to check + """ + return num if num > 0.0 else 0.0 + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + + def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) + + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +def create_vehicle_blueprint(world, actor_filter, ego=False, color=None, number_of_wheels=[4]): + """Create the blueprint for a specific actor type. + + Args: + actor_filter: a string indicating the actor type, e.g, 'vehicle.lincoln*'. + + Returns: + bp: the blueprint object of carla. + """ + blueprints = list(world.get_blueprint_library().filter(actor_filter)) + + blueprint_library = [] + for nw in number_of_wheels: + blueprint_library = blueprint_library + [x for x in blueprints if + int(x.get_attribute('number_of_wheels')) == nw] + bp = random.choice(blueprint_library) + if bp.has_attribute('color'): + if color is None: + color = random.choice(bp.get_attribute('color').recommended_values) + bp.set_attribute('color', color) + if bp.has_attribute('driver_id'): + driver_id = random.choice(bp.get_attribute('driver_id').recommended_values) + bp.set_attribute('driver_id', driver_id) + if not ego: + bp.set_attribute('role_name', 'autopilot') + else: + bp.set_attribute('role_name', 'hero') + + # bp.set_attribute('sticky_control', False) + return bp \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/render.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/render.py new file mode 100644 index 0000000000..ed4c5f1420 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/render.py @@ -0,0 +1,451 @@ +import carla +import weakref +import datetime +import os,sys +import collections +import math,random +import random,pygame +import numpy as np +from carla import ColorConverter as cc +from gym_carla.multi_lane.util.sensor import GnssSensor,LaneInvasionSensor,CollisionSensor +from gym_carla.multi_lane.util.misc import find_weather_presets,get_actor_display_name +# + +class World(object): + """ Class representing the surrounding environment """ + + def __init__(self,carla_world, hud): + """Constructor method""" + self.world = carla_world + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self,player): + """Restart the world""" + self.player=player + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 + + # # Get a random blueprint. + # blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + # blueprint.set_attribute('role_name', 'hero') + # if blueprint.has_attribute('color'): + # color = random.choice(blueprint.get_attribute('color').recommended_values) + # blueprint.set_attribute('color', color) + + # # Spawn the player. + # if self.player is not None: + # spawn_point = self.player.get_transform() + # spawn_point.location.z += 2.0 + # spawn_point.rotation.roll = 0.0 + # spawn_point.rotation.pitch = 0.0 + # self.destroy() + # self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # self.modify_vehicle_physics(self.player) + # while self.player is None: + # if not self.map.get_spawn_points(): + # print('There are no spawn points available in your map/town.') + # print('Please add some Vehicle Spawn Point to your UE4 scene.') + # sys.exit(1) + # spawn_points = self.map.get_spawn_points() + # spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + # self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # self.modify_vehicle_physics(self.player) + + self.world.tick() + + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + ] + for actor in actors: + if actor is not None: + actor.destroy() + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + #self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name.split('/')[-1], + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % transform.location.z, + ''] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ('Throttle:', control.throttle, 0.0, 1.0), + ('Steer:', control.steer, -1.0, 1.0), + ('Brake:', control.brake, 0.0, 1.0), + ('Reverse:', control.reverse), + ('Hand brake:', control.hand_brake), + ('Manual:', control.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ('Speed:', control.speed, 0.0, 5.556), + ('Jump:', control.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + + def dist(l): + return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) + ** 2 + (l.z - transform.location.z)**2) + vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + #self.help.render(display) + + + +class FadingText(object): + """ Class for fading text """ + + def __init__(self, font, dim, pos): + """Constructor method""" + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + """Set fading text""" + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + """Fading text method for every tick""" + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + """Render fading text method""" + display.blit(self.surface, self.pos) + + + +class HelpText(object): + """ Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + + + +class CameraManager(object): + """ Class for camera management""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform( + carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=1.6, z=1.7)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + blp.set_attribute('image_size_x', str(hud.dim[0])) + blp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + blp.set_attribute('range', '50') + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = True if self.index is None else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + """Render method""" + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/sensor.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/sensor.py new file mode 100644 index 0000000000..00513709d7 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/sensor.py @@ -0,0 +1,170 @@ +import collections +import logging +import weakref, math +import carla, time +from enum import Enum +from gym_carla.multi_lane.util.misc import get_actor_display_name, create_vehicle_blueprint + + +# +class SemanticTags(Enum): + NONE = 0 + Roads = 1 + Sidewalks = 2 + Buildings = 3 + Walls = 4 + Fences = 5 + Poles = 6 + TrafficLight = 7 + TrafficSigns = 8 + Vegetation = 9 + Terrain = 10 + Sky = 11 + Pedestrians = 12 + Rider = 13 + Car = 14 + Truck = 15 + Bus = 16 + Train = 17 + Motorcycle = 18 + Bicycle = 19 + Static = 20 + Dynamic = 21 + Other = 22 + Water = 23 + RoadLines = 24 + Ground = 25 + Bridge = 26 + RailTrack = 27 + GuardRail = 28 + Any = 255 + + +class CollisionSensor(object): + """Class for collision sensors""" + + def __init__(self, parent_actor, hud=None): + self.sensor = None + self.history = [] + self.hud = hud + self._parent = parent_actor + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_ref = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_ref, event)) + + def __del__(self): + self.history.clear() + self.sensor = None + + def get_collision_history(self): + """Get the histroy of collisions""" + history = collections.defaultdict(int) + tags = set() + for tag, frame, intensity in self.history: + history[frame] += intensity + tags.add(tag) + if self.hud: + # used in pypgam + return history + else: + # used elsewhere + return history, tags + + def destroy(self): + self.sensor.stop() + self.sensor.destroy() + + def clear_history(self): + self.history.clear() + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + if self.hud is not None: + self.hud.notification('Collision with %r' % actor_type) + # else: + # logging.info('Collision with %r',actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) + for tag in event.other_actor.semantic_tags: + self.history.append((SemanticTags(tag), event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +class LaneInvasionSensor(object): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud=None) -> None: + self.sensor = None + self._parent = parent_actor + self.count = 0 + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + def __del__(self): + self.count = 0 + self.sensor = None + + def destroy(self): + self.sensor.stop() + self.sensor.destroy() + + def get_invasion_count(self): + return self.count + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + self = weak_self() + if not self: + return + self.count += 1 + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + if self.hud is not None: + self.hud.notification('Crossed line %s' % ' and '.join(text)) + # else: + # logging.info('Crossed line %s' % ' and '.join(text)) + + +class GnssSensor(object): + """ Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), + attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/wrapper.py b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/wrapper.py new file mode 100644 index 0000000000..4937b1a5dc --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/multi_lane/util/wrapper.py @@ -0,0 +1,325 @@ +import carla +import math +import numpy as np +from enum import Enum +from gym_carla.multi_lane.util.misc import get_speed,get_yaw_diff,test_waypoint,get_sign +## +class WaypointWrapper: + """The location left, right, center is allocated according to the lane of ego vehicle""" + def __init__(self,opt=None) -> None: + self.left_front_wps=None + self.left_rear_wps=None + self.center_front_wps=None + self.center_rear_wps=None + self.right_front_wps=None + self.right_rear_wps=None + + if opt is not None: + if 'left_front_wps' in opt: + self.left_front_wps=opt['left_front_wps'] + if 'left_rear_wps' in opt: + self.left_rear_wps=opt['left_rear_wps'] + if 'center_front_wps' in opt: + self.center_front_wps=opt['center_front_wps'] + if 'center_rear_wps' in opt: + self.center_rear_wps=opt['center_rear_wps'] + if 'right_front_wps' in opt: + self.right_front_wps=opt['right_front_wps'] + if 'right_rear_wps' in opt: + self.right_rear_wps=opt['right_rear_wps'] + + +class VehicleWrapper: + """The location left, right, center is allocated according to the lane of ego vehicle""" + def __init__(self,opt=None) -> None: + self.left_front_veh=None + self.left_rear_veh=None + self.center_front_veh=None + self.center_rear_veh=None + self.right_front_veh=None + self.right_rear_veh=None + """distance sequence: + distance_to_front_vehicles:[left_front_veh,center_front_veh,right_front_veh] + distance_to_rear_vehicles:[left_rear_veh,center_rear_veh,right_rear_veh]""" + self.distance_to_front_vehicles=None + self.distance_to_rear_vehicles=None + + if opt is not None: + if 'left_front_veh' in opt: + self.left_front_veh=opt['left_front_veh'] + if 'left_rear_veh' in opt: + self.left_rear_veh=opt['left_rear_veh'] + if 'center_front_veh' in opt: + self.center_front_veh=opt['center_front_veh'] + if 'center_rear_veh' in opt: + self.center_rear_veh=opt['center_rear_veh'] + if 'right_front_veh' in opt: + self.right_front_veh=opt['right_front_veh'] + if 'right_rear_veh' in opt: + self.right_rear_veh=opt['right_rear_veh'] + if 'dis_to_front_vehs' in opt: + self.distance_to_front_vehicles=opt['dis_to_front_vehs'] + if 'dis_to_rear_vehs' in opt: + self.distance_to_rear_vehicles=opt['dis_to_rear_vehs'] + +class Truncated(Enum): + """Different truncate situations""" + FALSE=-1 + OTHER=0 + CHANGE_LANE_IN_LANE_FOLLOW=1 + COLLISION=2 + SPEED_LOW=3 + OUT_OF_ROAD=4 + OPPOSITE_DIRECTION=5 + TRAFFIC_LIGHT_BREAK=6 + CHANGE_TO_WRONG_LANE=7 + +class SpeedState(Enum): + """Different ego vehicle speed state + START: Initializing state, speed up the vehicle to speed_threshole, use basic agent controller + RUNNING: After initializing, ego speed between speed_min and speed_limit, use RL controller + REBOOT: After initializaing, ego speed reaches below speed min, use basic agent controller to speed up ego vehicle to speed_threshold + """ + START = 0 + RUNNING = 1 + RUNNING_RL = 2 + RUNNING_PID = 3 + +class Action(Enum): + """Parametrized Action for P-DQN""" + LANE_FOLLOW=0 + LANE_CHANGE_LEFT=-1 + LANE_CHANGE_RIGHT=1 + STOP=2 + +class ControlInfo: + """Wrapper for vehicle(model3) control info""" + def __init__(self,throttle=0.0,brake=0.0,steer=0.0,gear=1) -> None: + self.throttle=throttle + self.steer=steer + self.brake=brake + self.gear=gear + self.reverse=False + self.manual_gear_shift=False + +def process_lane_wp(wps_list, ego_vehicle_z, ego_forward_vector, my_sample_ratio, lane_offset): + wps = [] + idx = 0 + + # for wp in wps_list: + # delta_z = wp.transform.location.z - ego_vehicle_z + # yaw_diff = math.degrees(get_yaw_diff(wp.transform.get_forward_vector(), ego_forward_vector)) + # yaw_diff = yaw_diff / 90 + # if idx % my_sample_ratio == my_sample_ratio-1: + # wps.append([delta_z/2, yaw_diff, lane_offset]) + # idx = idx + 1 + # return np.array(wps) + for i in range(10): + wp = wps_list[i] + delta_z = wp.transform.location.z - ego_vehicle_z + yaw_diff = math.degrees(get_yaw_diff(wp.transform.get_forward_vector(), ego_forward_vector)) + yaw_diff = yaw_diff / 90 + wps.append([delta_z/3, yaw_diff, lane_offset]) + return np.array(wps) + + +def process_veh(ego_vehicle, vehs_info, left_wall, right_wall,vehicle_proximity): + vehicle_inlane=[vehs_info.left_front_veh,vehs_info.center_front_veh,vehs_info.right_front_veh, + vehs_info.left_rear_veh,vehs_info.center_rear_veh,vehs_info.right_rear_veh] + ego_speed = get_speed(ego_vehicle, False) + ego_location = ego_vehicle.get_location() + ego_bounding_x = ego_vehicle.bounding_box.extent.x + ego_bounding_y = ego_vehicle.bounding_box.extent.y + all_v_info = [] + print('vehicle_inlane: ', vehicle_inlane) + for i in range(6): + if i == 0 or i == 3: + lane = -1 + elif i == 1 or i == 4: + lane = 0 + else: + lane = 1 + veh = vehicle_inlane[i] + wall = False + if left_wall and (i == 0 or i == 3): + wall = True + if right_wall and (i == 2 or i == 5): + wall = True + if wall: + if i < 3: + v_info = [0.001, 0, lane] + else: + v_info = [-0.001, 0, lane] + else: + if veh is None: + if i < 3: + v_info = [1, 0, lane] + else: + v_info = [-1, 0, lane] + else: + veh_speed = get_speed(veh, False) + rel_speed = ego_speed - veh_speed + + distance = ego_location.distance(veh.get_location()) + vehicle_len = max(abs(ego_bounding_x), abs(ego_bounding_y)) + \ + max(abs(veh.bounding_box.extent.x), abs(veh.bounding_box.extent.y)) + distance -= vehicle_len + + if distance < 0: + if i < 3: + v_info = [0.001, rel_speed, lane] + else: + v_info = [-0.001, -rel_speed, lane] + else: + if i < 3: + v_info = [distance / vehicle_proximity, rel_speed, lane] + else: + v_info = [-distance / vehicle_proximity, -rel_speed, lane] + all_v_info.append(v_info) + # print(all_v_info) + return np.array(all_v_info) + +def process_steer(a_index, steer): + # left: steering is negative[-1, -0.1], right: steering is positive[0.1, 1], the thereshold here is sifnificant and it correlates with pdqn + processed_steer = steer + if a_index == 0: + processed_steer = steer * 0.5 - 0.5 + elif a_index == 2: + processed_steer = steer * 0.5 + 0.5 + return processed_steer + +def recover_steer(a_index, steer): + # recovery [-1, 1] from left change and right change + recovered_steer=steer + if a_index==0: + recovered_steer=(steer+0.5)/0.5 + elif a_index ==2: + recovered_steer=(steer-0.5)/0.5 + recovered_steer=np.clip(recovered_steer,-1,1) + return recovered_steer + +def fill_action_param(action, steer, throttle_brake, action_param, modify_change_steer): + if not modify_change_steer: + action_param[0][action*2] = steer + action_param[0][action*2+1] = throttle_brake + else: + if action == 0: + steer=recover_steer(action,steer) + elif action == 2: + steer=recover_steer(action,steer) + action_param[0][action*2] = steer + action_param[0][action*2+1] = throttle_brake + return action_param + +def ttc_reward(ego_veh,target_veh,min_dis,TTC_THRESHOLD): + """Caculate the time left before ego vehicle collide with target vehicle""" + #TTC = float('inf') + TTC=TTC_THRESHOLD + if target_veh and ego_veh: + distance = ego_veh.get_location().distance(target_veh.get_location()) + vehicle_len = max(abs(ego_veh.bounding_box.extent.x), + abs(ego_veh.bounding_box.extent.y)) + \ + max(abs(target_veh.bounding_box.extent.x), + abs(target_veh.bounding_box.extent.y)) + distance -= vehicle_len + # rel_speed = get_speed(ego_veh,False) - get_speed(target_veh, False) + # if abs(rel_speed) > float(0.0000001): + # TTC = distance / rel_speed + if distance < min_dis: + TTC = 0.01 + else: + distance -= min_dis + rel_speed = get_speed(ego_veh,False) - get_speed(target_veh, False) + if abs(rel_speed) > float(0.0000001): + TTC = distance / rel_speed + # fTTC=-math.exp(-TTC) + if TTC >= 0 and TTC <= TTC_THRESHOLD: + fTTC = np.clip(np.log(TTC / TTC_THRESHOLD), -1, 0) + else: + fTTC = 0 + #TTC=TTC_THRESHOLD + + return TTC,fTTC + +def comfort(fps, last_acc, acc, last_yaw, yaw): + acc_jerk = -((acc - last_acc) * fps) ** 2 / ((6 * fps) ** 2) + yaw_diff = math.degrees(get_yaw_diff(last_yaw, yaw)) + Yaw_jerk = -abs(yaw_diff) / 30 + return np.clip(acc_jerk * 0.5 + Yaw_jerk, -1, 0), yaw_diff + +def lane_center_reward(lane_center, ego_location): + def compute(center,ego): + Lcen=ego.distance(center.transform.location) + center_yaw=lane_center.transform.get_forward_vector() + dis=carla.Vector3D(ego.x-lane_center.transform.location.x, + ego.y-lane_center.transform.location.y,0) + Lcen*=get_sign(dis,center_yaw) + return Lcen + + if not test_waypoint(lane_center, True): + Lcen = 2.1 + fLcen = -2 + print('lane_center.lane_id, lane_center.road_id, flcen, lane_wid/2: ', lane_center.lane_id, + lane_center.road_id, fLcen, lane_center.lane_width / 2) + else: + Lcen =compute(lane_center,ego_location) + fLcen = -abs(Lcen)/(lane_center.lane_width/2) + # if self.current_action == Action.LANE_CHANGE_LEFT and self.current_lane == self.last_lane: + # # change left + # center_width=lane_center.lane_width + # lane_center=lane_center.get_left_lane() + # if lane_center is None: + # Lcen = 7 + # fLcen = -2 + # else: + # Lcen =compute(lane_center,ego_location) + # fLcen = -abs(Lcen) / (lane_center.lane_width/2+center_width) + # elif self.current_action == Action.LANE_CHANGE_RIGHT and self.current_lane == self.last_lane: + # #change right + # center_width=lane_center.lane_width + # lane_center=lane_center.get_right_lane() + # if lane_center is None: + # Lcen = 7 + # fLcen = -2 + # else: + # Lcen =compute(lane_center,ego_location) + # fLcen=-abs(Lcen)/(lane_center.lane_width/2+center_width) + # else: + # #lane follow and stop mode + # Lcen =compute(lane_center,ego_location) + # fLcen = -abs(Lcen)/(lane_center.lane_width/2) + #print('pdqn_lane_center: Lcen, fLcen: ', Lcen, fLcen) + return Lcen, fLcen + +def calculate_guide_lane_center(ego_location, lane_center, location, front_distance, rear_distance): + Lcen = lane_center.transform.location.distance(ego_location) + # print( + # f"Lane Center:{Lcen}, Road ID:{lane_center.road_id}, Lane ID:{lane_center.lane_id}, Yaw:{self.ego_vehicle.get_transform().rotation.yaw}") + if not test_waypoint(lane_center, True) or Lcen > lane_center.lane_width / 2 + 0.1: + fLcen = -2 + print('lane_center.lane_id, lcen, flcen: ', lane_center.lane_id, lane_center.road_id, Lcen, fLcen, + lane_center.lane_width / 2) + else: + left = False + right = False + if lane_center.lane_id != -1 and front_distance[0] > 20 and front_distance[0]/front_distance[1] > 1.2 and rear_distance[0] > 20: + left = True + if lane_center.lane_id != -3 and front_distance[2] > 20 and front_distance[2]/front_distance[1] > 1.2 and rear_distance[2] > 20: + right = True + if left: + Lcen = lane_center.get_left_lane().transform.location.distance(location) + fLcen = - Lcen / lane_center.lane_width + elif right: + Lcen = lane_center.get_right_lane().transform.location.distance(location) + fLcen = - Lcen / lane_center.lane_width + else: + Lcen = lane_center.transform.location.distance(ego_location) + # print( + # f"Lane Center:{Lcen}, Road ID:{lane_center.road_id}, Lane ID:{lane_center.lane_id}, Yaw:{self.ego_vehicle.get_transform().rotation.yaw}") + if not test_waypoint(lane_center, True) or Lcen > lane_center.lane_width / 2 + 0.1: + fLcen = -2 + print('lane_center.lane_id, lcen, flcen: ', lane_center.lane_id, lane_center.road_id, Lcen, fLcen, lane_center.lane_width / 2) + else: + fLcen = - Lcen / (lane_center.lane_width / 2) + return Lcen, fLcen + diff --git a/src/distributed_hierarchical_attentive/gym_carla/setting.py b/src/distributed_hierarchical_attentive/gym_carla/setting.py new file mode 100644 index 0000000000..8263b704d0 --- /dev/null +++ b/src/distributed_hierarchical_attentive/gym_carla/setting.py @@ -0,0 +1 @@ +CARLA_PATH = 'D:\CARLA_0.9.14\WindowsNoEditor'# \ No newline at end of file diff --git a/src/distributed_hierarchical_attentive/main/tester/multi_lane_test.py b/src/distributed_hierarchical_attentive/main/tester/multi_lane_test.py new file mode 100644 index 0000000000..29c14f288d --- /dev/null +++ b/src/distributed_hierarchical_attentive/main/tester/multi_lane_test.py @@ -0,0 +1,274 @@ +import logging +import torch +import datetime +import random, os +import numpy as np +import scipy.io as sio +import matplotlib.pyplot as plt +from tqdm import tqdm +from algs.pdqn import P_DQN +from collections import deque +from tensorboardX import SummaryWriter +from gym_carla.multi_lane.settings import ARGS +from gym_carla.multi_lane.carla_env import CarlaEnv, SpeedState +from main.util.process import start_process, kill_process + +# +# neural network hyper parameters +SIGMA = 1 +SIGMA_STEER = 0.3 +SIGMA_ACC = 0.5 +THETA = 0.001 +DEVICE = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu') +LR_ACTOR = 0.001 +LR_CRITIC = 0.002 +GAMMA = 0.99 # q值更新系数 +TAU = 0.01 # 软更新参数 +EPSILON = 0.5 # epsilon-greedy +POLICY_UPDATE_FREQ = 5 +BUFFER_SIZE = 20000 +MINIMAL_SIZE = 10000 +BATCH_SIZE = 128 +REPLACE_A = 500 +REPLACE_C = 300 +TOTAL_EPISODE = 3000 +clip_grad = 10 +zero_index_gradients = True +inverting_gradients = True +base_name = f'origin_NOCA' +time = datetime.datetime.now().strftime('%Y%m%d%H%M%S') +SAVE_PATH = f"./out/multi_lane/pdqn/test/{time}" +if not os.path.exists(SAVE_PATH): + os.makedirs(SAVE_PATH) + + +def main(): + ARGS.set_defaults(train=False) + ARGS.set_defaults(no_rendering=False) + args = ARGS.parse_args() + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + # env=gym.make('CarlaEnv-v0') + env = CarlaEnv(args) + globals()['modify_change_steer'] = args.modify_change_steer + + done = False + truncated = False + + random.seed(0) + torch.manual_seed(8) + s_dim = env.get_observation_space() + a_bound = env.get_action_bound() + a_dim = 2 + + episode_writer = SummaryWriter(SAVE_PATH) + result = [] + + for run in [base_name]: + param = torch.load('./out/pdqn_final_6.pth') + agent = P_DQN(s_dim, a_dim, a_bound, GAMMA, TAU, SIGMA_STEER, SIGMA, SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, + BATCH_SIZE, LR_ACTOR, + LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients, False, DEVICE) + agent.load_net(param) + agent.train = False + env.RL_switch = True + agent.set_sigma(0, 0) + + VEL = [] + ACC = [] + JERK = [] + OFFLANE = [] + TTC = [] + REAR_VEL = [] + REAR_ACC = [] + + try: + for i in range(30): + state = env.reset() + + score = 0 + ttc, efficiency, comfort, lcen, yaw, impact, lane_change_reward = 0, 0, 0, 0, 0, 0, 0 # part objective scores + recover_time, lane_change_count, brake_count, global_brake_count, delay_index, avg_vel, rec_avg_vel, avg_jerk, avg_offlane = 0, 0, 0, 0, 0, 0, 0, 0, 0 + delay_i = deque(maxlen=100) + rear_a = deque(maxlen=5) + rear_v = deque(maxlen=200) + recovery_mode = False + ego_v = deque(maxlen=5000) + ego_a = deque(maxlen=5000) + ego_jerk = deque(maxlen=5000) + ego_offlane = deque(maxlen=5000) + ego_ttc = deque(maxlen=5000) + ego_rear_a = deque(maxlen=5000) + ego_rear_v = deque(maxlen=5000) + recover_v = deque(maxlen=500) + + while not done and not truncated: + action, action_param, all_action_param = agent.take_action(state) + next_state, reward, truncated, done, info = env.step(action, action_param) + # if env.speed_state == SpeedState.REBOOT: + # env.speed_state = SpeedState.RUNNING + state = next_state + print() + + if env.is_effective_action() and not info['Abandon']: + score += reward + if not truncated: + ttc += info['fTTC'] + efficiency += info['Efficiency'] + comfort += info['Comfort'] + lcen += info['Lane_center'] + yaw += info['Yaw'] + impact += info['impact'] + lane_change_reward += info['lane_changing_reward'] + if not truncated and not done: + # with open(f"{SAVE_PATH}/ego_ttc.txt",'a') as f: + # f.write(str(info['TTC'])+'\n') + # with open(f"{SAVE_PATH}/ego_vel.txt",'a') as f: + # f.write(str(info['velocity'])+'\n') + # with open(f"{SAVE_PATH}/ego_acc.txt",'a') as f: + # f.write(str(info['cur_acc'])+'\n') + # with open(f"{SAVE_PATH}/ego_jerk.txt",'a') as f: + # f.write(str(abs(info['cur_acc']-info['last_acc'])/(1.0/args.fps))+'\n') + # with open(f"{SAVE_PATH}/ego_offlane.txt",'a') as f: + # f.write(str(abs(info['offlane']))+'\n') + # with open(f"{SAVE_PATH}/ego_yaw.txt",'a') as f: + # f.write(str(info['yaw_diff'])+'\n') + + # macro index + if info['change_lane'] and info['rear_id'] != -1: + recovery_mode = True + lane_change_count += 1 + if info['rear_id'] != -1: + if info['rear_a'] < 0: + global_brake_count += 1 + ego_rear_a.append((info['rear_a'], recovery_mode)) + # with open(f"{SAVE_PATH}/rear_a.txt",'a') as f: + # f.write(str(recovery_mode)+'\t'+str(info['rear_a'])+'\n') + if recovery_mode == True: + rear_a.append(info['rear_a']) + if info['rear_id'] == -1: + avg_v = 0 + for n in range(len(rear_v) - 1): + avg_v += rear_v[n + 1] / (len(rear_v) - 1) + ind = rear_v[0] / (avg_v + 0.000001) if rear_v[0] / (avg_v + 0.000001) > 1.0 else 1.0 + recover_v.append(avg_v) + delay_i.append(ind) + + recovery_mode = False + rear_v.clear() + rear_a.clear() + elif len(rear_a) == rear_a.maxlen: + avg_a = 0 + for a in rear_a: + avg_a += a / rear_a.maxlen + if 0 <= avg_a < 0.001: + rear_v.append(info['rear_v']) + avg_v = 0 + for n in range(len(rear_v) - 1): + avg_v += rear_v[n + 1] / (len(rear_v) - 1) + ind = rear_v[0] / (avg_v + 0.000001) if rear_v[0] / ( + avg_v + 0.000001) > 1.0 else 1.0 + delay_i.append(ind) + + recovery_mode = False + rear_v.clear() + rear_a.clear() + + if recovery_mode == True: + if info['rear_a'] < 0: + brake_count += 1 + rear_v.append(info['rear_v']) + recover_time += 1 + + # micro index + ego_v.append(info['velocity']) + ego_a.append(info['cur_acc']) + ego_jerk.append(abs(info['cur_acc'] - info['last_acc']) / (1.0 / args.fps)) + ego_offlane.append(info['offlane']) + ego_ttc.append(info['TTC']) + ego_rear_v.append(info['rear_v']) + + if done: + episode_writer.add_scalar('Pass_Time_Steps', env.time_step, i) + if done or truncated: + # restart the training + done = False + truncated = False + + VEL.append(np.array(ego_v, dtype=float)) + ACC.append(np.array(ego_a, dtype=float)) + JERK.append(np.array(ego_jerk, dtype=float)) + OFFLANE.append(np.array(ego_offlane, dtype=float)) + TTC.append(np.array(ego_ttc, dtype=float)) + REAR_ACC.append(np.array(ego_rear_a, dtype=float)) + REAR_VEL.append(np.array(ego_rear_v, dtype=float)) + + episode_writer.add_scalar('Total_Reward', score, i) + score /= env.time_step + 1 + episode_writer.add_scalar('Avg_Reward', score, i) + episode_writer.add_scalar('Time_Steps', env.time_step, i) + episode_writer.add_scalar('TTC', ttc / (env.time_step + 1), i) + episode_writer.add_scalar('Efficiency', efficiency / (env.time_step + 1), i) + episode_writer.add_scalar('Comfort', comfort / (env.time_step + 1), i) + episode_writer.add_scalar('Lcen', lcen / (env.time_step + 1), i) + episode_writer.add_scalar('Yaw', yaw / (env.time_step + 1), i) + episode_writer.add_scalar('Impact', impact / (env.time_step + 1), i) + episode_writer.add_scalar('Lane_change_reward', lane_change_reward / (env.time_step + 1), i) + episode_writer.add_scalar('recover_time', recover_time, i) + episode_writer.add_scalar('lane_change_count', lane_change_count, i) + episode_writer.add_scalar('brake_count', brake_count, i) + episode_writer.add_scalar('global_brake_count', brake_count, i) + for index in delay_i: + delay_index += index / len(delay_i) + delay_index = delay_index if delay_index > 1.0 else 1.0 + episode_writer.add_scalar('delay_index', delay_index, i) + for vel in ego_v: + avg_vel += vel / len(ego_v) + episode_writer.add_scalar('average_vel', avg_vel, i) + for jerk in ego_jerk: + avg_jerk += jerk / len(ego_jerk) + episode_writer.add_scalar('average_jerk', avg_jerk, i) + for offlane in ego_offlane: + avg_offlane += abs(offlane) / len(ego_offlane) + episode_writer.add_scalar('average_offlane', avg_offlane, i) + if len(ego_ttc) > 0: + temp = [] + for ttc in ego_ttc: + if ttc < 0: + temp.append(args.TTC_th) + else: + temp.append(ttc) + min_ttc = min(temp) + else: + min_ttc = args.TTC_th + episode_writer.add_scalar('min_ttc', min_ttc, i) + for vel in recover_v: + rec_avg_vel += vel / len(recover_v) + episode_writer.add_scalar('rec_avg_vel', rec_avg_vel, i) + + np.save(f"{SAVE_PATH}/ego_vel.npy", np.array(VEL)) + np.save(f"{SAVE_PATH}/ego_acc.npy", np.array(ACC)) + np.save(f"{SAVE_PATH}/ego_jerk.npy", np.array(JERK)) + np.save(f"{SAVE_PATH}/ego_offlane.npy", np.array(OFFLANE)) + np.save(f"{SAVE_PATH}/ego_ttc.npy", np.array(TTC)) + np.save(f"{SAVE_PATH}/rear_acc.npy", np.array(REAR_ACC)) + np.save(f"{SAVE_PATH}/rear_vel.npy", np.array(REAR_VEL)) + except KeyboardInterrupt: + logging.info("Premature Terminated") + except BaseException as e: + logging.info(e.args) + finally: + env.__del__() + episode_writer.close() + logging.info('\nDone.') + + +if __name__ == '__main__': + try: + start_process() + main() + except BaseException as e: + logging.warning(e.args) + finally: + kill_process() diff --git a/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_lane.py b/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_lane.py new file mode 100644 index 0000000000..001600cd43 --- /dev/null +++ b/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_lane.py @@ -0,0 +1,361 @@ +import logging +import torch +import datetime, os, sys +import random, collections +import numpy as np +import matplotlib.pyplot as plt + +curPath = os.path.abspath(os.path.dirname(__file__)) +rootPath = os.path.split(os.path.split(curPath)[0])[0] +sys.path.append(rootPath) +from tqdm import tqdm +from collections import deque +from algs.pdqn import P_DQN +from tensorboardX import SummaryWriter +from gym_carla.multi_lane.settings import ARGS +from gym_carla.multi_lane.carla_env import CarlaEnv +from main.util.process import start_process, kill_process +from gym_carla.multi_lane.util.wrapper import fill_action_param, recover_steer, Action + +# +# neural network hyper parameters +SIGMA = 0.5 +SIGMA_STEER = 0.3 +SIGMA_ACC = 0.5 +THETA = 0.05 +DEVICE = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu') +LR_ACTOR = 0.0002 +LR_CRITIC = 0.0002 +GAMMA = 0.9 # q值更新系数 +TAU = 0.01 # 软更新参数 +EPSILON = 0.5 # epsilon-greedy +BUFFER_SIZE = 160000 +MINIMAL_SIZE = 40000 +BATCH_SIZE = 256 +REPLACE_A = 500 +REPLACE_C = 300 +TOTAL_EPISODE = 5000 +SIGMA_DECAY = 0.9999 +PER_FLAG = True +modify_change_steer = False +clip_grad = 10 +zero_index_gradients = True +inverting_gradients = True +base_name = f'origin_NOCA' +time = datetime.datetime.now().strftime('%Y%m%d%H%M%S') +SAVE_PATH = f"./out/multi_lane/pdqn/{time}" +if not os.path.exists(SAVE_PATH): + os.makedirs(SAVE_PATH) + + +def main(): + args = ARGS.parse_args() + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + # env=gym.make('CarlaEnv-v0') + env = CarlaEnv(args) + globals()['modify_change_steer'] = args.modify_change_steer + + done = False + truncated = False + + random.seed(0) + torch.manual_seed(16) + s_dim = env.get_observation_space() + a_bound = env.get_action_bound() + a_dim = 2 + + episode_writer = SummaryWriter(SAVE_PATH) + n_run = 3 + rosiolling_window = 100 # 100 car following events, average score + result = [] + + for run in [base_name]: + agent = P_DQN(s_dim, a_dim, a_bound, GAMMA, TAU, SIGMA_STEER, SIGMA, SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, + BATCH_SIZE, LR_ACTOR, + LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients, PER_FLAG, DEVICE) + + # training part + max_rolling_score = np.float32('-5') + max_score = np.float32('-30') + var = 3 + collision_train = 0 + episode_score = [] + rolling_score = [] + cum_collision_num = [] + + score_safe = [] + score_efficiency = [] + score_comfort = [] + + try: + for i in range(10): + with tqdm(total=TOTAL_EPISODE // 10, desc="Iteration %d" % i) as pbar: + for i_episode in range(TOTAL_EPISODE // 10): + state = env.reset() + agent.reset_noise() + score = 0 + ttc, efficiency, comfort, lcen, yaw, impact, lane_change_reward = 0, 0, 0, 0, 0, 0, 0 # part objective scores + impact_deque = deque(maxlen=2) + + recover_time, lane_change_count, brake_count, delay_index, avg_vel, avg_jerk, avg_lcen = 0, 0, 0, 0, 0, 0, 0 + delay_i = deque(maxlen=100) + rear_a = deque(maxlen=5) + rear_v = deque(maxlen=200) + recovery_mode = False + ego_v = deque(maxlen=1500) + ego_jerk = deque(maxlen=1500) + ego_lcen = deque(maxlen=1500) + ego_ttc = deque(maxlen=1500) + + while not done and not truncated: + action, action_param, all_action_param = agent.take_action(state) + next_state, reward, truncated, done, info = env.step(action, action_param) + if env.is_effective_action() and not info['Abandon']: + replay_buffer_adder(agent, impact_deque, state, next_state, all_action_param, reward, + truncated, done, info) + + if not truncated and not done: + # macro index + if info['change_lane'] and info['rear_id'] != -1: + recovery_mode = True + lane_change_count += 1 + if recovery_mode == True: + rear_a.append(info['rear_a']) + if info['rear_id'] == -1: + avg_v = 0 + for n in range(len(rear_v) - 1): + avg_v += rear_v[n + 1] / (len(rear_v) - 1) + ind = rear_v[0] / (avg_v + 0.000001) if rear_v[0] / ( + avg_v + 0.000001) > 1.0 else 1.0 + delay_i.append(ind) + + recovery_mode = False + rear_v.clear() + rear_a.clear() + elif len(rear_a) == rear_a.maxlen: + avg_a = 0 + for a in rear_a: + avg_a += a / rear_a.maxlen + if 0 <= avg_a < 0.001: + rear_v.append(info['rear_v']) + avg_v = 0 + for n in range(len(rear_v) - 1): + avg_v += rear_v[n + 1] / (len(rear_v) - 1) + ind = rear_v[0] / (avg_v + 0.000001) if rear_v[0] / ( + avg_v + 0.000001) > 1.0 else 1.0 + delay_i.append(ind) + + recovery_mode = False + rear_v.clear() + rear_a.clear() + + if recovery_mode == True: + if info['rear_a'] < 0: + brake_count += 1 + rear_v.append(info['rear_v']) + recover_time += 1 + + # micro index + ego_v.append(info['velocity']) + ego_jerk.append((info['cur_acc'] - info['last_acc']) / (1.0 / args.fps)) + ego_lcen.append(abs(info['offlane'])) + ego_ttc.append(info['TTC']) + + print( + f"state -- vehicle_info:{state['vehicle_info']}\n" + # f"waypoints:{state['left_waypoints']}, \n" + f"waypoints:{state['center_waypoints']}, \n" + # f"waypoints:{state['right_waypoints']}, \n" + f"ego_vehicle:{state['ego_vehicle']}, \n" + f"light info: {state['light']}\n" + f"next_state -- vehicle_info:{next_state['vehicle_info']}\n" + # f"waypoints:{next_state['left_waypoints']}, \n" + f"waypoints:{next_state['center_waypoints']}, \n" + # f"waypoints:{next_state['right_waypoints']}, \n" + f"ego_vehicle:{next_state['ego_vehicle']}\n" + f"light info: {next_state['light']}\n" + f"action:{action}, action_param:{action_param}, all_action_param:{all_action_param}\n" + f"reward:{reward}, truncated:{truncated}, done:{done}\n" + f"rear_id:{info['rear_id']}, rear_v:{info['rear_v']}, rear_a:{info['rear_a']}, " + f"time_step:{info['time_step']}, change_lane:{info['change_lane']}, " + f"recover_time:{recover_time}, lane_change_count:{lane_change_count}, brake_count:{brake_count}, delay_index:{delay_index}") + print() + + if agent.replay_buffer.size() >= MINIMAL_SIZE: + logging.info("Learn begin: %f %f", SIGMA_STEER, SIGMA_ACC) + # alter the batch_size and update times according to the replay buffer size: + # reference: https://zhuanlan.zhihu.com/p/345353294, https://arxiv.org/abs/1711.00489 + k = agent.replay_buffer.size() // MINIMAL_SIZE + agent.batch_size = k * BATCH_SIZE + [agent.learn() for _ in range(k)] + + state = next_state + if env.is_effective_action() and not info['Abandon']: + score += reward + if not truncated: + ttc += info['fTTC'] + efficiency += info['Efficiency'] + comfort += info['Comfort'] + lcen += info['Lane_center'] + yaw += info['Yaw'] + impact += info['impact'] + lane_change_reward += info['lane_changing_reward'] + + if env.total_step == args.pre_train_steps: + agent.save_net(f"{SAVE_PATH}/pdqn_pre_trained.pth") + + if env.rl_control_step > 10000 and env.is_effective_action() and \ + env.RL_switch and SIGMA_ACC > 0.01: + globals()['SIGMA'] *= SIGMA_DECAY + globals()['SIGMA_STEER'] *= SIGMA_DECAY + globals()['SIGMA_ACC'] *= SIGMA_DECAY + agent.set_sigma(SIGMA_STEER, SIGMA_ACC) + + if done or truncated: + # restart the training + done = False + truncated = False + + # record episode results + if env.RL_switch: + episode_writer.add_scalar('Total_Reward', score, i * (TOTAL_EPISODE // 10) + i_episode) + score /= env.time_step + 1 + episode_writer.add_scalar('Avg_Reward', score, i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Time_Steps', env.time_step, + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('TTC', ttc / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Efficiency', efficiency / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Comfort', comfort / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Lcen', lcen / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Yaw', yaw / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Impact', impact / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Lane_change_reward', lane_change_reward / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + + episode_score.append(score) + score_safe.append(ttc) + score_efficiency.append(efficiency) + score_comfort.append(comfort) + # rolling_score.append(np.mean(episode_score[max])) + cum_collision_num.append(collision_train) + + if max_score < score: + max_score = score + agent.save_net(F"{SAVE_PATH}/pdqn_optimal.pth") + + episode_writer.add_scalar('recover_time', recover_time, i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('lane_change_count', lane_change_count, + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('brake_count', brake_count, i * (TOTAL_EPISODE // 10) + i_episode) + for index in delay_i: + delay_index += index / len(delay_i) + delay_index = delay_index if delay_index > 1.0 else 1.0 + episode_writer.add_scalar('delay_index', delay_index, i * (TOTAL_EPISODE // 10) + i_episode) + for vel in ego_v: + avg_vel += vel / len(ego_v) + episode_writer.add_scalar('average_vel', avg_vel, i * (TOTAL_EPISODE // 10) + i_episode) + for jerk in ego_jerk: + avg_jerk += jerk / len(ego_jerk) + episode_writer.add_scalar('average_jerk', avg_jerk, i * (TOTAL_EPISODE // 10) + i_episode) + for lcen in ego_lcen: + avg_lcen += lcen / len(ego_lcen) + episode_writer.add_scalar('average_lcen', avg_lcen, i * (TOTAL_EPISODE // 10) + i_episode) + if len(ego_ttc) > 0: + min_ttc = min(ego_ttc) + else: + min_ttc = args.TTC_th + episode_writer.add_scalar('min_ttc', min_ttc, i * (TOTAL_EPISODE // 10) + i_episode) + + """ if rolling_score[rolling_score.__len__-1]>max_rolling_score: + max_rolling_score=rolling_score[rolling_score.__len__-1] + agent.save_net() """ + + # result.append([episode_score,rolling_score,cum_collision_num,score_safe,score_efficiency,score_comfort]) + if (i_episode + 1) % 10 == 0: + pbar.set_postfix({ + 'episodes': '%d' % (TOTAL_EPISODE / 10 * i + i_episode + 1), + 'score': '%.2f' % score + }) + pbar.update(1) + agent.save_net(f"{SAVE_PATH}/pdqn_final.pth") + + np.save(f"{SAVE_PATH}/result_{run}.npy", result) + except KeyboardInterrupt: + logging.info("Premature Terminated") + # except BaseException as e: + # logging.info(e.args) + finally: + env.__del__() + episode_writer.close() + agent.save_net(f"{SAVE_PATH}/pdqn_final.pth") + logging.info('\nDone.') + + +def replay_buffer_adder(agent, impact_deque, state, next_state, all_action_param, reward, truncated, done, info): + """Input all the state info into agent's replay buffer""" + if 'Throttle' in info: + control_state = info['control_state'] + throttle_brake = -info['Brake'] if info['Brake'] > 0 else info['Throttle'] + if info['Change'] == Action.LANE_FOLLOW: + action = 1 + elif info['Change'] == Action.LANE_CHANGE_LEFT: + action = 0 + elif info['Change'] == Action.LANE_CHANGE_RIGHT: + action = 2 + # action_param = np.array([[info['Steer'], throttle_brake]]) + saved_action_param = fill_action_param(action, info['Steer'], throttle_brake, + all_action_param, modify_change_steer) + print(f"Control In Replay Buffer: {action}, {saved_action_param}") + if control_state: + # under rl control + if truncated: + agent.store_transition(state, action, saved_action_param, reward, next_state, + truncated, done, info) + else: + impact = info['impact'] / 9 + impact_deque.append([state, action, saved_action_param, reward, next_state, + truncated, done, info]) + if len(impact_deque) == 2: + experience = impact_deque[0] + agent.store_transition(experience[0], experience[1], experience[2], + experience[3] + impact, experience[4], experience[5], + experience[6], experience[7]) + # agent.replay_buffer.add(state, action, saved_action_param, reward, next_state, + # truncated, done, info) + else: + # Input the guided action to replay buffer + if truncated: + agent.store_transition(state, action, saved_action_param, reward, next_state, + truncated, done, info) + else: + impact = info['impact'] / 9 + impact_deque.append([state, action, saved_action_param, reward, next_state, + truncated, done, info]) + if len(impact_deque) == 2: + experience = impact_deque[0] + agent.store_transition(experience[0], experience[1], experience[2], + experience[3] + impact, experience[4], experience[5], + experience[6], experience[7]) + # agent.replay_buffer.add(state, action, saved_action_param, reward, next_state, + # truncated, done, info) + # else: + # # not work + # # Input the agent action to replay buffer + # agent.replay_buffer.add(state, action, all_action_param, reward, next_state, truncated, done, info) + + +if __name__ == '__main__': + try: + start_process() + main() + # except BaseException as e: + # logging.warning(e.args) + finally: + kill_process() diff --git a/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_process.py b/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_process.py new file mode 100644 index 0000000000..0cd79101db --- /dev/null +++ b/src/distributed_hierarchical_attentive/main/trainer/pdqn_multi_process.py @@ -0,0 +1,384 @@ +import logging +import torch +import datetime, time, os +import random, collections +import numpy as np +import matplotlib.pyplot as plt +import multiprocessing as mp +from tqdm import tqdm +from copy import deepcopy +from collections import deque +from algs.pdqn import P_DQN +from tensorboardX import SummaryWriter +from multiprocessing import Process, Queue, Pipe, connection +from gym_carla.multi_lane.settings import ARGS +from gym_carla.multi_lane.carla_env import CarlaEnv +from main.util.process import start_process, kill_process +from gym_carla.multi_lane.util.wrapper import fill_action_param, recover_steer, Action + +# +# neural network hyper parameters +SIGMA = 0.5 +SIGMA_STEER = 0.3 +SIGMA_ACC = 0.5 +THETA = 0.05 +DEVICE = torch.device('cuda') if torch.cuda.is_available() else torch.device('cpu') +LR_ACTOR = 0.0002 +LR_CRITIC = 0.0002 +GAMMA = 0.9 # q值更新系数 +TAU = 0.01 # 软更新参数 +EPSILON = 0.5 # epsilon-greedy +BUFFER_SIZE = 160000 +MINIMAL_SIZE = 10000 +BATCH_SIZE = 128 +REPLACE_A = 500 +REPLACE_C = 300 +TOTAL_EPISODE = 7000 +SIGMA_DECAY = 0.9999 +PER_FLAG = True +modify_change_steer = False +clip_grad = 10 +zero_index_gradients = True +inverting_gradients = True +base_name = f'origin_NOCA' +time = datetime.datetime.now().strftime('%Y%m%d%H%M%S') +SAVE_PATH = f"./out/multi_agent/pdqn/{time}" + + +def main(): + Args = ARGS.parse_args() + log_level = logging.DEBUG if Args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + # env=gym.make('CarlaEnv-v0') + env = CarlaEnv(Args) + globals()['modify_change_steer'] = Args.modify_change_steer + + done = False + truncated = False + + random.seed(0) + torch.manual_seed(16) + s_dim = env.get_observation_space() + a_bound = env.get_action_bound() + a_dim = 2 + + episode_writer = SummaryWriter(SAVE_PATH) + n_run = 3 + rosiolling_window = 100 # 100 car following events, average score + result = [] + + for run in [base_name]: + worker_agent = P_DQN(deepcopy(s_dim), a_dim, a_bound, GAMMA, TAU, SIGMA_STEER, SIGMA, SIGMA_ACC, THETA, EPSILON, + BUFFER_SIZE, BATCH_SIZE, LR_ACTOR, + LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients, PER_FLAG, + torch.device('cpu')) + # learner_agent = P_DQN(deepcopy(s_dim), a_dim, a_bound, GAMMA, TAU, SIGMA_STEER, SIGMA, SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, BATCH_SIZE, LR_ACTOR, + # LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients,PER_FLAG, DEVICE) + + # multi-process training + process = list() + traj_q = Queue(maxsize=BUFFER_SIZE) + agent_q = Queue(maxsize=3) + learner_q = Queue(maxsize=1) + traj_send, traj_recv = Pipe() + agent_send, agent_recv = Pipe() + mp.set_start_method(method='spawn', force=True) # force all the multiprocessing to 'spawn' methods + # process.append(mp.Process(target=learner_mp,args=(traj_recv,agent_send,(deepcopy(s_dim), a_dim, a_bound),Args.ego_num))) + process.append(mp.Process(target=learner_mp, args=(traj_q, agent_q, (deepcopy(s_dim), a_dim, a_bound)))) + # process.append(mp.Process(target=learner_mp,args=(traj_q,agent_q,learner_q,(deepcopy(s_dim), a_dim, a_bound)))) + [p.start() for p in process] + + # training part + max_rolling_score = np.float32('-5') + max_score = np.float32('-30') + var = 3 + collision_train = 0 + learn_time = 0 + episode_score = [] + rolling_score = [] + cum_collision_num = [] + + score_safe = [] + score_efficiency = [] + score_comfort = [] + + try: + for i in range(10): + with tqdm(total=TOTAL_EPISODE // 10, desc="Iteration %d" % i) as pbar: + for i_episode in range(TOTAL_EPISODE // 10): + state = env.reset() + worker_agent.reset_noise() + score = 0 + ttc, efficiency, comfort, lcen, yaw, impact, lane_change_reward = 0, 0, 0, 0, 0, 0, 0 # part objective scores + + while not done and not truncated: + # if agent_recv.poll(): + # a,a_t,c,c_t=agent_recv.recv() + # worker_agent.actor.load_state_dict(a) + # worker_agent.actor_target.load_state_dict(a_t) + # worker_agent.critic.load_state_dict(c) + # worker_agent.critic_target.load_state_dict(c_t) + if not agent_q.empty(): + ac, actor_t, cr, critic_t = worker_agent.actor.state_dict(), worker_agent.actor_target.state_dict(), \ + worker_agent.critic.state_dict(), worker_agent.critic_target.state_dict() + # temp_agent,learn_time=agent_q.get() + # a,c=temp_agent.actor.state_dict(),temp_agent.critic.state_dict() + # worker_agent.actor.load_state_dict(temp_agent.actor.state_dict()) + # worker_agent.critic.load_state_dict(temp_agent.critic.state_dict()) + actor, critic, learn_time = agent_q.get() + worker_agent.actor = actor + worker_agent.critic = critic + + action, action_param, all_action_param = worker_agent.take_action(state) + next_state, reward, truncated, done, info = env.step(action, action_param) + if env.is_effective_action() and not info['Abandon']: + throttle_brake = -info['Brake'] if info['Brake'] > 0 else info['Throttle'] + if info['Change'] == Action.LANE_FOLLOW: + action = 1 + elif info['Change'] == Action.LANE_CHANGE_LEFT: + action = 0 + elif info['Change'] == Action.LANE_CHANGE_RIGHT: + action = 2 + # action_param = np.array([[info['Steer'], throttle_brake]]) + saved_action_param = fill_action_param(action, info['Steer'], throttle_brake, + all_action_param, modify_change_steer) + print(f"Control In Replay Buffer: {action}, {saved_action_param}") + # traj_send.send((j,states[j],next_states[j],all_action_params[j], + # rewards[j],truncateds[j],dones[j],infos[j])) + # if not traj_q.full(): + traj_q.put((deepcopy(state), deepcopy(next_state), deepcopy(action), + deepcopy(saved_action_param), deepcopy(reward), + deepcopy(truncated), deepcopy(done), deepcopy(info)), block=True, + timeout=None) + + print( + f"state -- vehicle_info:{state['vehicle_info']}\n" + # f"waypoints:{state['left_waypoints']}, \n" + # f"waypoints:{state['center_waypoints']}, \n" + # f"waypoints:{state['right_waypoints']}, \n" + f"ego_vehicle:{state['ego_vehicle']}, \n" + f"light info: {state['light']}\n" + f"next_state -- vehicle_info:{next_state['vehicle_info']}\n" + # f"waypoints:{next_state['left_waypoints']}, \n" + # f"waypoints:{next_state['center_waypoints']}, \n" + # f"waypoints:{next_state['right_waypoints']}, \n" + f"ego_vehicle:{next_state['ego_vehicle']}\n" + f"light info: {next_state['light']}\n" + f"action:{action}, action_param:{action_param} \n" + f"all_action_param:{all_action_param}, saved_action_param:{saved_action_param}\n" + f"reward:{reward}, truncated:{truncated}, done:{done}, learn_time:{learn_time}") + print() + + state = next_state + + # only record the first vehicle reward + if env.total_step == Args.pre_train_steps: + worker_agent.save_net(f"{SAVE_PATH}/pdqn_pre_trained.pth") + if env.is_effective_action() and not info['Abandon']: + score += reward + if not truncated: + ttc += info['fTTC'] + efficiency += info['Efficiency'] + comfort += info['Comfort'] + lcen += info['Lane_center'] + yaw += info['Yaw'] + impact += info['impact'] + lane_change_reward += info['lane_changing_reward'] + + if env.rl_control_step > 10000 and env.is_effective_action() and \ + env.RL_switch and SIGMA_ACC > 0.1: + globals()['SIGMA'] *= SIGMA_DECAY + globals()['SIGMA_STEER'] *= SIGMA_DECAY + globals()['SIGMA_ACC'] *= SIGMA_DECAY + worker_agent.set_sigma(SIGMA_STEER, SIGMA_ACC) + logging.info("Agent Sigma %f %f", SIGMA_STEER, SIGMA_ACC) + + if done or truncated: + # restart the training + done = False + truncated = False + + # record episode results + if env.RL_switch: + episode_writer.add_scalar('Total_Reward', score, i * (TOTAL_EPISODE // 10) + i_episode) + score /= env.time_step + 1 + episode_writer.add_scalar('Avg_Reward', score, i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Time_Steps', env.time_step, + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('TTC', ttc / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Efficiency', efficiency / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Comfort', comfort / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Lcen', lcen / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Yaw', yaw / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Impact', impact / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + episode_writer.add_scalar('Lane_change_reward', lane_change_reward / (env.time_step + 1), + i * (TOTAL_EPISODE // 10) + i_episode) + + episode_score.append(score) + score_safe.append(ttc) + score_efficiency.append(efficiency) + score_comfort.append(comfort) + # rolling_score.append(np.mean(episode_score[max])) + cum_collision_num.append(collision_train) + + if max_score < score: + max_score = score + worker_agent.save_net(F"{SAVE_PATH}/pdqn_optimal.pth") + + """ if rolling_score[rolling_score.__len__-1]>max_rolling_score: + max_rolling_score=rolling_score[rolling_score.__len__-1] + agent.save_net() """ + + # result.append([episode_score,rolling_score,cum_collision_num,score_safe,score_efficiency,score_comfort]) + if (i_episode + 1) % 10 == 0: + pbar.set_postfix({ + 'episodes': '%d' % (TOTAL_EPISODE / 10 * i + i_episode + 1), + 'score': '%.2f' % score + }) + pbar.update(1) + worker_agent.save_net(f"{SAVE_PATH}/pdqn_final.pth") + + np.save(f"{SAVE_PATH}/result_{run}.npy", result) + except KeyboardInterrupt: + logging.info("Premature Terminated") + # except BaseException as e: + # logging.info(e.args) + finally: + env.__del__() + # process[-1].join() # waiting for learner + episode_writer.close() + worker_agent.save_net(f"{SAVE_PATH}/pdqn_final.pth") + # process[-1].join() + process_safely_terminate(process) + logging.info('\nDone.') + + +def process_safely_terminate(process: list): + for p in process: + try: + p.kill() + except OSError as e: + logging.SystemError(e) + + +# Pipe version multiprocess +# def learner_mp(traj_recv:connection.Connection, agent_send:connection.Connection, agent_param, ego_num): +# learner_agent=P_DQN(agent_param[0], agent_param[1], agent_param[2], GAMMA, TAU, SIGMA_STEER, SIGMA, SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, BATCH_SIZE, LR_ACTOR, +# LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients,PER_FLAG, DEVICE) +# impact_deques=[deque(maxlen=2) for _ in range(ego_num)] +# while(True): +# if traj_recv.poll(timeout=None): +# trajectory=traj_recv.recv() +# ego_id, state, next_state, all_action_param, reward, truncated, done, info=trajectory[0],trajectory[1],trajectory[2],trajectory[3],\ +# trajectory[4],trajectory[5],trajectory[6],trajectory[7] +# replay_buffer_adder(learner_agent,impact_deques[ego_id],state,next_state,all_action_param,reward,truncated,done,info) + +# if learner_agent.replay_buffer.size()>=MINIMAL_SIZE: +# logging.info("LEARN BEGIN") +# learner_agent.learn() +# if learner_agent.learn_time!=0 and learner_agent.learn_time%2==0: +# actor,actor_t,critic,critic_t=learner_agent.actor.state_dict(),learner_agent.actor_target.state_dict(), \ +# learner_agent.critic.state_dict(),learner_agent.critic_target.state_dict() +# a,a_t,c,c_t=deepcopy(learner_agent.actor.state_dict()),deepcopy(learner_agent.actor_target.state_dict()),\ +# deepcopy(learner_agent.critic.state_dict()),deepcopy(learner_agent.critic_target.state_dict()) +# agent_send.send((a,a_t,c,c_t)) + +# Queue vesion multiprocess +def learner_mp(traj_q: Queue, agent_q: Queue, agent_param): + learner_agent = P_DQN(deepcopy(agent_param[0]), agent_param[1], agent_param[2], GAMMA, TAU, SIGMA_STEER, SIGMA, + SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, BATCH_SIZE, LR_ACTOR, + LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients, PER_FLAG, DEVICE) + temp_agent = P_DQN(deepcopy(agent_param[0]), agent_param[1], agent_param[2], GAMMA, TAU, SIGMA_STEER, SIGMA, + SIGMA_ACC, THETA, EPSILON, BUFFER_SIZE, BATCH_SIZE, LR_ACTOR, + LR_CRITIC, clip_grad, zero_index_gradients, inverting_gradients, PER_FLAG, torch.device('cpu')) + impact_deque = deque(maxlen=2) + pid = os.getpid() + actor, actor_t, critic, critic_t = None, None, None, None + a, a_t, c, c_t = None, None, None, None + while (True): + # alter the batch_size and update times according to the replay buffer size: + # reference: https://zhuanlan.zhihu.com/p/345353294, https://arxiv.org/abs/1711.00489 + k = max(learner_agent.replay_buffer.size() // MINIMAL_SIZE, 1) + learner_agent.batch_size = k * BATCH_SIZE + for _ in range(BATCH_SIZE): + trajectory = traj_q.get(block=True, timeout=None) + state, next_state, action, saved_action_param, reward, truncated, done, info = trajectory[0], trajectory[1], \ + trajectory[2], trajectory[3], \ + trajectory[4], trajectory[5], trajectory[6], trajectory[7] + replay_buffer_adder(learner_agent, impact_deque, state, next_state, action, saved_action_param, reward, + truncated, done, info) + if learner_agent.replay_buffer.size() >= MINIMAL_SIZE: + logging.info("LEARN BEGIN") + # print(f"LEARN TIME:{learner_agent.learn_time}") + [learner_agent.learn() for _ in range(k)] + if not agent_q.full(): + actor = deepcopy(learner_agent.actor).to('cpu') + actor_t = deepcopy(learner_agent.actor_target).to('cpu') + critic = deepcopy(learner_agent.critic).to('cpu') + critic_t = deepcopy(learner_agent.critic_target).to('cpu') + temp_agent.actor.load_state_dict(learner_agent.actor.state_dict()) + temp_agent.critic.load_state_dict(learner_agent.critic.state_dict()) + # actor,actor_t,critic,critic_t=learner_agent.actor.state_dict(),learner_agent.actor_target.state_dict(), \ + # learner_agent.critic.state_dict(),learner_agent.critic_target.state_dict() + # a,a_t,c,c_t=temp_agent.actor.state_dict(),temp_agent.actor_target.state_dict(), \ + # temp_agent.critic.state_dict(),temp_agent.critic_target.state_dict() + agent_q.put((actor, critic, learner_agent.learn_time), block=True, timeout=None) + # agent_q.put((temp_agent,learner_agent.learn_time),block=True,timeout=None) + + +def replay_buffer_adder(agent, impact_deque, state, next_state, action, saved_action_param, reward, truncated, done, + info): + """Input all the state info into agent's replay buffer""" + if 'Throttle' in info: + if info['control_state']: + # under rl control + if truncated: + agent.store_transition(state, action, saved_action_param, reward, next_state, + truncated, done, info) + else: + impact = info['impact'] / 9 + impact_deque.append([state, action, saved_action_param, reward, next_state, + truncated, done, info]) + if len(impact_deque) == 2: + experience = impact_deque[0] + agent.store_transition(experience[0], experience[1], experience[2], + experience[3] + impact, experience[4], experience[5], + experience[6], experience[7]) + # agent.replay_buffer.add(state, action, saved_action_param, reward, next_state, + # truncated, done, info) + else: + # Input the guided action to replay buffer + if truncated: + agent.store_transition(state, action, saved_action_param, reward, next_state, + truncated, done, info) + else: + impact = info['impact'] / 9 + impact_deque.append([state, action, saved_action_param, reward, next_state, + truncated, done, info]) + if len(impact_deque) == 2: + experience = impact_deque[0] + agent.store_transition(experience[0], experience[1], experience[2], + experience[3] + impact, experience[4], experience[5], + experience[6], experience[7]) + # agent.replay_buffer.add(state, action, saved_action_param, reward, next_state, + # truncated, done, info) + # else: + # # not work + # # Input the agent action to replay buffer + # agent.replay_buffer.add(state, action, all_action_param, reward, next_state, truncated, done, info) + + +if __name__ == '__main__': + try: + start_process() + main() + # except BaseException as e: + # logging.warning(e.args) + finally: + kill_process() diff --git a/src/distributed_hierarchical_attentive/main/util/process.py b/src/distributed_hierarchical_attentive/main/util/process.py new file mode 100644 index 0000000000..ad9098675c --- /dev/null +++ b/src/distributed_hierarchical_attentive/main/util/process.py @@ -0,0 +1,44 @@ +import os,psutil,time +import subprocess +from gym_carla.setting import CARLA_PATH + +operating_system='windows' if os.name=='nt' else 'linux' + +def get_binary(): + return 'CarlaUE4.exe' if operating_system=='windows' else 'CarlaUE4.sh' + +def get_exec_command(): + binary=get_binary() + exec_command=binary if operating_system=='windows' else ('./'+binary) + + return binary,exec_command +# +def kill_process(): + binary=get_binary() + for process in psutil.process_iter(): + if process.name().lower().startswith(binary.split('.')[0].lower()): + try: + process.terminate() + except: + pass + + still_alive=[] + for process in psutil.process_iter(): + if process.name().lower().startswith(binary.split('.')[0].lower()): + still_alive.append(process) + + if len(still_alive): + for process in still_alive: + try: + process.kill() + except: + pass + psutil.wait_procs(still_alive) + +# Starts Carla simulator +def start_process(): + # Kill Carla processes if there are any and start simulator + print('Starting Carla...') + kill_process() + subprocess.Popen(get_exec_command()[1],cwd=CARLA_PATH, shell=True) + time.sleep(5) \ No newline at end of file