| 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: |