I have a robot control program. The robot receives the coordinates of the target and, knowing its coordinates (x, y, theta), must turn to the target. To rotate, it needs to send the command self.geometry_msg.angular.z with a negative value (to rotate clockwise) or positive (counterclockwise). The question is how to understand which way to turn him? Now he does not always correctly select the shortest angle of rotation. How to fix it? Formula with math.atan2 found on the Internet.

angle = math.atan2(y_goal - self.y, x_goal - self.x) - self.theta if abs(angle) > 0.1: self.geometry_msg.angular.z = 0.5*angle/abs(angle) self.geometry_msg.linear.x = 0.3 else: self.geometry_msg.angular.z = 0 self.geometry_msg.linear.x = 1 self.velocity_publisher.publish(self.geometry_msg)

x_goal - x target coordinate

y_goal - y coordinate of the target

self.x - the x coordinate of the robot

self.y - y coordinate of the robot

self.theta - rotation angle of the robot relative to OX

Theta angle is counted from 0 to 6.28 (counterclockwise) and up to -6.28 (hourly)

abs (angle)> 0.1 this condition can be changed, I just do not need a high accuracy of rotation, quite an accuracy of 0.1 (in radians).

  • 3
    1. If you rotate by 270 degrees, where will you rotate? That's right, you need to lay down the condition that if the angle of rotation is more than 180 (3.14 happy), or less than -180 (-3.14 happy) then do 360 minus "your degree". - nick_n_a
  • one
    2. Since artangence does not work well at small (if not mistaken) degrees. It is necessary to add a condition that on the interval where the art tangles "badly" thinks, take 90+ arc tangents and unfold x and y, i.e. for half corners, math.atan2(y_goal - self.y, x_goal - self.x) and for the second arcctg2( x_goal - self.x, y_goal - self.y) . it will add to you accuracy. Perhaps this fact in the library took into account ... and maybe not. - nick_n_a
  • one
    and you can turn self.theta into a directing vector, then the sign of the coordinate axis Z of the vector product of this vector will give the desired direction of rotation. - Fat-Zer

0