State of the trajectory.
| 112 | |
| 113 | @dataclass |
| 114 | class State(CertesianState, FrenetState): |
| 115 | """State of the trajectory.""" |
| 116 | |
| 117 | def __post_init__(self): |
| 118 | if self.vel == 0 and self.s_d != 0: |
| 119 | self.vel = math.sqrt(self.s_d**2 + self.d_d**2) |
| 120 | |
| 121 | """ |
| 122 | Modified from: https://blog.csdn.net/u013468614/article/details/108748016 |
| 123 | """ |
| 124 | |
| 125 | def complete_cartesian2D(self, rx: float, ry: float, ryaw: float, |
| 126 | rkappa: float) -> None: |
| 127 | cos_theta_r = math.cos(ryaw) |
| 128 | sin_theta_r = math.sin(ryaw) |
| 129 | |
| 130 | self.x = rx - sin_theta_r * self.d |
| 131 | self.y = ry + cos_theta_r * self.d |
| 132 | if self.s_d <= 1e-1: |
| 133 | self.s_d = 1e-1 |
| 134 | self.vel = 0 |
| 135 | self.yaw = None |
| 136 | else: |
| 137 | one_minus_kappa_r_d = 1 - rkappa * self.d |
| 138 | self.vel = math.sqrt(one_minus_kappa_r_d**2 * self.s_d**2 + |
| 139 | self.d_d**2) |
| 140 | self.yaw = math.asin(self.d_d / self.vel) + ryaw |
| 141 | return |
| 142 | |
| 143 | def complete_frenet2D(self, rs: float, rx: float, ry: float, ryaw: float, |
| 144 | rkappa: float) -> None: |
| 145 | self.s = rs |
| 146 | dx = self.x - rx |
| 147 | dy = self.y - ry |
| 148 | |
| 149 | cos_theta_r = math.cos(ryaw) |
| 150 | sin_theta_r = math.sin(ryaw) |
| 151 | cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx |
| 152 | self.d = math.copysign(math.sqrt(dx * dx + dy * dy), cross_rd_nd) |
| 153 | |
| 154 | delta_theta = self.yaw - ryaw |
| 155 | sin_delta_theta = math.sin(delta_theta) |
| 156 | cos_delta_theta = math.cos(delta_theta) |
| 157 | one_minus_kappa_r_d = 1 - rkappa * self.d |
| 158 | self.s_d = self.vel * cos_delta_theta / one_minus_kappa_r_d |
| 159 | self.d_d = self.vel * sin_delta_theta |
| 160 | return |
| 161 | |
| 162 | |
| 163 | @dataclass |
no outgoing calls
no test coverage detected