(agent, agents, pref_vel)
| 1100 | |
| 1101 | |
| 1102 | def get_ttc_vel(agent, agents, pref_vel): |
| 1103 | try: |
| 1104 | if agent: |
| 1105 | vel_to_exe = pref_vel |
| 1106 | if not vel_to_exe: # path is not ready. |
| 1107 | return None |
| 1108 | |
| 1109 | speed_to_exe = agent.preferred_speed |
| 1110 | for other_agent in agents: |
| 1111 | if other_agent and agent.actor.id != other_agent.actor.id: |
| 1112 | s_f = get_velocity(other_agent.actor).length() |
| 1113 | d_f = (get_position(other_agent.actor) - get_position(agent.actor)).length() |
| 1114 | d_safe = 5.0 |
| 1115 | a_max = 3.0 |
| 1116 | s = max(0, s_f * s_f + 2 * a_max * (d_f - d_safe))**0.5 |
| 1117 | speed_to_exe = min(speed_to_exe, s) |
| 1118 | |
| 1119 | cur_vel = get_velocity(agent.actor) |
| 1120 | angle_diff = get_signed_angle_diff(vel_to_exe, cur_vel) |
| 1121 | if angle_diff > 30 or angle_diff < -30: |
| 1122 | vel_to_exe = 0.5 * (vel_to_exe + cur_vel) |
| 1123 | |
| 1124 | vel_to_exe = vel_to_exe.make_unit_vector() * speed_to_exe |
| 1125 | |
| 1126 | return vel_to_exe |
| 1127 | except Exception as e: |
| 1128 | print(e) |
| 1129 | |
| 1130 | return None |
| 1131 | |
| 1132 | |
| 1133 |
no test coverage detected