A weird "yaw" behavior in simulation using atan2() function

Question

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 yaw 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.

Here I've posted a video. Take a look on its behavior when it across 360°.

Ok guys here the complete function now:

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 < 0.0f )  // * read later on
    yaw += 2.0f * M_PI;

 output_trajectory.orientation = tf::createQuaternionMsgFromYaw( yaw );

  return output_trajectory;
}

where tf::createQuaternionMsgFromYaw is a library from the ROS framework. Here the defintion: link. geometry_msgs::Pose is simply a container: link.

*: here I've read related topics and questions here in stackoverflow and this function maps the returned output of atan2 into 0°-360°

UPDATE: here an extract from the yaw value:

...
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
...

As you can see the across point is "continuos", but it turns from 174° to 186° not in the right (the smallest) direction.

What i expect is that the quadrotor moves by small adjustments and rotatinng all around 360° insted of a few degree.

How can I get rid of this problem? I need a smooth yaw movement in my application. Regards

Answer 1

I don't think atan gives you the right angle. Atan gives -pi/2 ~ +pi/2 ranged results.

If you want to get exact angle in radians, you might have to write something like this (which i did before, worked nice):

// First find the section in which your coordinate is, then add the needed (x*pi) value to result:
double result = atan2(..);
if((x2 - x1 > 0) && (y2 - y1 > 0)){
  //section = 1;
  result += 0;
}
else if((x2 - x1 < 0) && (y2 - y1 > 0)){
   //section = 2;
  result += pi;
}
else if((x2 - x1 < 0) && (y2 - y1 < 0)){
   //section = 3
  result += pi;
}
else if((x2 - x1 > 0) && (y2 - y1 > 0)){
   //section = 4
  result += 2*pi;
}
else if(x2 == x1){
    if(y2 > y1){result = pi/2);
    if(y1 > y2){result = -pi/2);
}
else if(y2 == y1){
   if(x2 > x1){result = 0;}
   if(x1 > x1){result = pi;}
}
else if((x1 == x2) && (y1 == y2)){
   std::cout << "This is not line, just a point\n"; // :P
}

Answer 2

Ok. I got it. After hours of investigation I realized that the problem it not related to atan2() function or some sign change of the angle when it comes to jump over 180° or 360°.

Reading carefully the following code as as example

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    const double degree = M_PI/180;

    // robot state
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] ="swivel";
        joint_state.position[0] = swivel;
        joint_state.name[1] ="tilt";
        joint_state.position[1] = tilt;
        joint_state.name[2] ="periscope";
        joint_state.position[2] = height;


        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // Create new robot state
        tilt += tinc;
        if (tilt<-.5 || tilt>0) tinc *= -1;
        height += hinc;
        if (height>.2 || height<0) hinc *= -1;
        swivel += degree;
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;
}

which I found here I realized that the variable angle is incremented every time of a small amount and then passed to the quaternion library tf::createQuaternionMsgFromYaw() This means 2 things:

1. You never take care of the jump across 180° or 360°;
2. more important thing, you never pass an absolute angle. For istance you never call in your code tf::createQuaternionMsgFromYaw(degtorad(179)) and then tf::createQuaternionMsgFromYaw(degtorad(182)), but tf::createQuaternionMsgFromYaw(angle + delta_angle);

Regards