Skip to content

Commit

Permalink
check for big yaw diffences
Browse files Browse the repository at this point in the history
  • Loading branch information
Johannes-Thiel committed Sep 18, 2024
1 parent e62f377 commit 7823f10
Showing 1 changed file with 3 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,9 @@ async def _drive(self, distance: float) -> None:
# flip if it is pointing backwards
if np.abs(yaw - self.odometer.prediction.yaw) > math.pi / 2:
yaw = yaw + math.pi

if np.abs(yaw - self.odometer.prediction.yaw) > 0.2:
self.log.error(f'Yaw difference is too high: {np.abs(yaw - self.odometer.prediction.yaw)}')
return
fitted_line = rosys.geometry.Line(a=m, b=-1, c=c)
closest_point = fitted_line.foot_point(self.odometer.prediction.point)
target = rosys.geometry.Pose(x=closest_point.x, y=closest_point.y, yaw=yaw)
Expand Down

0 comments on commit 7823f10

Please sign in to comment.