Closed zhoulaifu closed 5 years ago
seems like this behavior occurs also with smaller linear velocity values, the y-axis change is just too small to notice. i believe thats the root cause of this issue: https://github.com/ros/ros_tutorials/blob/2b1de43ba413bf59bbb7eb0578bf79e1721049de/turtlesim/src/turtle.cpp#L158 so as you can see even if orient_ is zero, still there will be movement on the y-axis. ill try to provide a PR soon...
While the drawn line isn't very intuitive I don't think the result is wrong. Whenever the turtle hits the limit of the screen the position is clamped back into the visible range. In this case the distance is extremely large in x direction and due to floating point inaccuracy significant enough in y direction to travel to the border of the screen.
Since turtlesim
is targeted to be a simple tutorial of ROS functionality I don't think it needs to handle this extreme input data anyhow different.
Therefore I will go ahead and close this ticket for now. Please feel free to continue commenting on it.
I believe that the FP inaccuracy can be largely reduced if, in ros_tutorials/turtlesim/src/turtle.cpp, we change
pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;
pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;
to
pos_.rx() += std::cos(orient_) * lin_vel_ * dt;
pos_.ry() += - std::sin(orient_) * lin_vel_ * dt;
This is based on the mathematical facts that sin(x+pi/2)=cos(x) and cos(x+pi/2)=-sin(x). See https://en.wikipedia.org/wiki/List_of_trigonometric_identities for example.
The proposed patch will indeed make the starting configuration better. But for any move with extreme speeds after a rotation the problem stays the same, see e.g. the result of the following sequence:
turtle_teleop_key
: once forwardturtle_teleop_key
: once left turnturtle_teleop_key
: once forwardrostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[10000000000000000,0,0]' '[0,0,0]'
You would expect the last segment of the drawn line to be in the same direction as the second last segement.
If you would like to create a PR with your proposed change I am happy to merge it. I just don't see the point if it only addresses one special case.
The screenshot you showed above (the one with turtle_teleop_key
) should be as expected (and therefore, should not invalidate the proposed fix IMHO). If you review line160-168 of ros_tutorials/turtlesim/src/turtle.cpp:
// Clamp to screen size
if (pos_.x() < 0 || pos_.x() > canvas_width ||
pos_.y() < 0 || pos_.y() > canvas_height)
{
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y());
}
pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), static_cast<double>(canvas_width)));
pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), static_cast<double>(canvas_height)));
To be sure, you can try commenting out the two lines with pos_.setX and pos_setY. I got this screenshot with your sequences of commands above:
The simulated turtle moves in a wrong direction if the linear velocity is very large. This issue can be reproduced with the following steps.
roscore
.turtlesim_node
with "rosrun turtlesim turtlesim_node`./turtle1/cmd_vel
topic:rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[10000000000000000,0,0]' '[0,0,0]'
.It can be seen that the published message is a velocity of type
geometry_msgs/Twist
. This velocity comes with a large linear speed in the x-axis, and 0 in the y-axis and z-axis. Thus, the simulated turtle is expected to move horizontally. In the current implementation, however, the turtle moves to the bottom-right direction (see screenshot below). Also, If we change the "10000000000000000" part in step 3 to a smaller number, say, "100000000", then the simulated turtle will move horizontally as expected.The issue, in my humble opinion, is due to floating-point inaccuracy. I can provide a pull request if needed.