Skip to content

Commit

Permalink
Use double when handling qreal orient_
Browse files Browse the repository at this point in the history
When the value of `orient_` crosses the boundary of `float`, e.g., when
`cmd_vel.angular.z = -4.0e+214`, `normalizeAngle` returns `nan`.
As a result, `x`, `y`, and `theta` all become `nan`, and turtle disappears
from the frame.

As `qreal` corresponds to `double`, `normalizeAngle` should be taking param
`angle` as `double`, and return `double` accordingly.
  • Loading branch information
squizz617 authored and paudrow committed Dec 20, 2021
1 parent d4f4cc5 commit a7c2d9e
Showing 1 changed file with 3 additions and 3 deletions.
6 changes: 3 additions & 3 deletions turtlesim/src/turtle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
namespace turtlesim
{

static float normalizeAngle(float angle)
static double normalizeAngle(double angle)
{
return angle - (TWO_PI * std::floor((angle + PI) / (TWO_PI)));
}
Expand Down Expand Up @@ -206,8 +206,8 @@ bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image,
}
else
{
float theta = normalizeAngle(rotate_absolute_goal_handle_->get_goal()->theta);
float remaining = normalizeAngle(theta - static_cast<float>(orient_));
double theta = normalizeAngle(rotate_absolute_goal_handle_->get_goal()->theta);
double remaining = normalizeAngle(theta - static_cast<float>(orient_));

// Update result
rotate_absolute_result_->delta = normalizeAngle(static_cast<float>(rotate_absolute_start_orient_ - orient_));
Expand Down

0 comments on commit a7c2d9e

Please sign in to comment.