title: A weird "yaw" behavior in simulation using atan2() function
body: <p>I'm working on an application, a simulator, where a quadrotor flies from waypoint to waypoint.
  In my code I've implemented a function to calculate the <a href="http://en.wikipedia.org/wiki/Yaw_%28rotation%29" rel="nofollow">yaw</a> using atan2 function. 
  But when the quadrotor turns over 360° it doesn't move through the shortest way but it move all around the 360° range to reached the new direction.</p>
  
  <p>Here I've posted a <a href="https://www.youtube.com/watch?v=GP5IZzruELw" rel="nofollow">video</a>. Take a look on its behavior when it across 360°.</p>
  
  <p>Ok guys here the complete function now:</p>
  
  <pre><code>geometry_msgs::Pose getYaw( double x1, double x2, double y1, double y2 ) {
  
  geometry_msgs::Pose output_trajectory;
  
  /* Extrapolate the yaw information between two contigous points */
  double yaw = atan2( ( y2 - y1 ), ( x2 - x1 ) );
  
    if( yaw &lt; 0.0f )  // * read later on
      yaw += 2.0f * M_PI;
  
   output_trajectory.orientation = tf::createQuaternionMsgFromYaw( yaw );
  
    return output_trajectory;
  }
  </code></pre>
  
  <p>where tf::createQuaternionMsgFromYaw is a library from the ROS framework. Here the defintion: <a href="http://docs.ros.org/diamondback/api/tf/html/c++/transform__datatypes_8h.html" rel="nofollow">link</a>.
  geometry_msgs::Pose is simply a container: <a href="http://docs.ros.org/indigo/api/geometry_msgs/html/msg/Pose.html" rel="nofollow">link</a>.</p>
  
  <p>*: here I've read related topics and questions here in stackoverflow and this function maps the returned output of atan2 into 0°-360°</p>
  
  <p>UPDATE:
  here an extract from the yaw value:</p>
  
  <pre><code>...
  Yaw: 131.3678
  Yaw: 133.3495
  Yaw: 135.6426
  Yaw: 138.3442
  Yaw: 141.5859
  Yaw: 145.5487
  Yaw: 150.4813
  Yaw: 156.7167
  Yaw: 164.6657
  Yaw: 174.7288
  Goal reached
  Moving to the 3 waypoint
  Yaw: 174.7288
  Yaw: 186.4225
  Yaw: 196.3789
  Yaw: 204.1349
  Yaw: 210.1296
  Yaw: 214.7946
  Yaw: 218.4716
  Yaw: 221.4110
  Yaw: 223.7921
  Yaw: 225.7431
  Yaw: 227.3565
  ...
  </code></pre>
  
  <p>As you can see the across point is "continuos", but it turns from 174° to 186° not in the right (the smallest) direction.</p>
  
  <p>What i expect is that the quadrotor moves by small adjustments and rotatinng all around 360° insted of a few degree.</p>
  
  <p>How can I get rid of this problem? I need a smooth yaw movement in my application.
  Regards</p>