Strange yaw behavior in a simulation using the atan2 () function

Question

I am working on an application, a simulator, where the quadrotor flies from point to point. In my code, I implemented a function to compute yaw using the atan2 function. But when the quadrotor rotates 360 °, it does not move along the shortest path, but it moves around 360 ° to reach a new direction.

Here I posted a video. Take a look at his behavior when it is 360 °.

Ok guys now the full function:

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 the library from the ROS structure. Here's the job: link . geometry_msgs :: A pose is just a container: a link .

*: here I read related topics and questions here on stackoverflow, and this function displays the returned atan2 output at 0 ° -360 °

UPDATE: here is an excerpt 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 intersection is “continuity”, but it does not turn right from 174 ° to 186 ° (the smallest).

What I expect is that the quadrotor moves with small settings and rotates around 360 ° several degrees.

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

Answer 1

Ok I understood. After several hours of research, I realized that the problem is not related to the atan2 () function or some sign of a change in angle when jumping 180 ° or 360 °.

Read the following code carefully as an 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 a variable angle increases every time a small amount, and then passed to the quaternion library tf::createQuaternionMsgFromYaw() This means 2 things:

• You will never take care of jumping 180 ° or 360 °;
• The more important thing, you never go through an absolute angle. For istance, you never call tf :: createQuaternionMsgFromYaw (degtorad (179)) in your code and then tf :: createQuaternionMsgFromYaw (degtorad (182)), but tf :: createQuaternionMsgFromYaw (angle + delta_angle);

Hi

Answer 2

I do not think that Atan gives you the right angle. Atan gives the results as -pi / 2 ~ + pi / 2.

If you want to get the exact angle in radians, you might have to write something like this (what I did before worked fine):

 // 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 }