Orientation Incorrect on GPS Duro

[zeerek-polymath]

On my GPS we are getting West North Up orientation (which does not match the standard right hand or left hand ) with inverted rotation instead of the ROS standard East North Up.

In code it may be related to the code here:

duro_gps_driver/src/duro.cpp

Lines 230 to 248 in 67db9dc

	void orientation_callback(u16 sender_id, u8 len, u8 msg[], void *context)
	{
	// enable MSG ID 544 in swift console
	// the MSG ID comes from eg #define SBP_MSG_ORIENT_QUAT 0x0220 --> 544
	msg_orient_quat_t *orimsg = (msg_orient_quat_t *)msg;
	double w = orimsg->w * pow(2, -31);
	double x = orimsg->x * pow(2, -31);
	double y = orimsg->y * pow(2, -31);
	double z = orimsg->z * pow(2, -31);
	tf2::Quaternion tf_orig(x, y, z, w);
	tf2::Quaternion tf_rot, tf_aligned;
	tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation
	tf_aligned = tf_rot * tf_orig; // left-handerd / right handed rotation
	pose_msg.pose.orientation.w = tf_aligned.w() * -1;
	pose_msg.pose.orientation.x = tf_aligned.y(); // left-handerd / right handed orientation
	pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation
	pose_msg.pose.orientation.z = tf_aligned.z(); // left-handerd / right handed orientation
	}

More specifically this code block:

pose_msg.pose.orientation.w = tf_aligned.w() * -1;
pose_msg.pose.orientation.x = tf_aligned.y();      // left-handerd / right handed orientation
pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation
pose_msg.pose.orientation.z = tf_aligned.z();      // left-handerd / right handed orientation

which inverts w and x and causes inverted rotation and West to be the X axis instead of ROS standard East.

Could we get a param to switch this behavior to keep current behavior possible set to true?

[horverno]

Sure, this is a good idea, early next week I will add param, so we can keep backward compatibility and also add this feature.

[horverno]

@zeerek-polymath i added the param to the 16-orientation-issue branch, please test it and share your comments

currently our GPS sensor is not available, but after the sensor test and your comments, it can be merged into master branch

[zeerek-polymath]

By default the raw values have us rotating about the x axis. By changing

duro_gps_driver/src/duro.cpp

Lines 244 to 249 in f00726f

	if(orientation_enu){
	pose_msg.pose.orientation.w = w;
	pose_msg.pose.orientation.x = x;
	pose_msg.pose.orientation.y = y;
	pose_msg.pose.orientation.z = z;
	}

to:

  if(orientation_enu){ 
    pose_msg.pose.orientation.w = w; 
    pose_msg.pose.orientation.x = x * -1;
    pose_msg.pose.orientation.y = y; 
    pose_msg.pose.orientation.z = z;   
  }

We get rotation correctly about Z. We need to then apply another rotation by PI_2 about Z to get ENU (without this rotation we our 0 rotation angle is SOUTH instead of EAST. However, direction of rotation is correct).

We are double checking our frames of reference and should have more of an idea tomorrow.

However, do you know why we have to multiply the x portion of the quaternion to rotate appropriately about z?

[horverno]

We used our other GPS as a reference (Novatel PWRPAK7 https://github.com/swri-robotics/novatel_gps_driver) and we determined the orientation accordingly.

As far as I understand the problem was with the left handed / right handed orientation.

Duro uses the following frames:

While in ROS Novatel uses the following:

[anshu3012]

On my GPS we are getting West North Up orientation (which does not match the standard right hand or left hand ) with inverted rotation instead of the ROS standard East North Up.

In code it may be related to the code here:

duro_gps_driver/src/duro.cpp

Lines 230 to 248 in 67db9dc

	void orientation_callback(u16 sender_id, u8 len, u8 msg[], void *context)
	{
	// enable MSG ID 544 in swift console
	// the MSG ID comes from eg #define SBP_MSG_ORIENT_QUAT 0x0220 --> 544
	msg_orient_quat_t *orimsg = (msg_orient_quat_t *)msg;
	double w = orimsg->w * pow(2, -31);
	double x = orimsg->x * pow(2, -31);
	double y = orimsg->y * pow(2, -31);
	double z = orimsg->z * pow(2, -31);
	tf2::Quaternion tf_orig(x, y, z, w);
	tf2::Quaternion tf_rot, tf_aligned;
	tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation
	tf_aligned = tf_rot * tf_orig; // left-handerd / right handed rotation
	pose_msg.pose.orientation.w = tf_aligned.w() * -1;
	pose_msg.pose.orientation.x = tf_aligned.y(); // left-handerd / right handed orientation
	pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation
	pose_msg.pose.orientation.z = tf_aligned.z(); // left-handerd / right handed orientation
	}

More specifically this code block:

pose_msg.pose.orientation.w = tf_aligned.w() * -1;
pose_msg.pose.orientation.x = tf_aligned.y();      // left-handerd / right handed orientation
pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation
pose_msg.pose.orientation.z = tf_aligned.z();      // left-handerd / right handed orientation

which inverts w and x and causes inverted rotation and West to be the X axis instead of ROS standard East.

Could we get a param to switch this behavior to keep current behavior possible set to true?

It seems like correcting from NED frame to ENU frame. Can someone help me understand the transformations?

[horverno]

@anshu3012 @zeerek-polymath

Let me illustrate the problem with small gifs. What you can observe is two gps-es mounted on the same vehicle. The orange arrow is Swiftnav Duro the green one is Novatel PWRPAK7 novatel driver. Novatel is a 20 Hz dual antenna RTK GPS, much more expensive compared to Duro (which provides only 10 Hz pos data).

• /gps/duro/current_pose orange arrow in rviz
• /gps/nova/current_pose green arrow in rviz

You can download some rosbags directly from here: https://jkk-research.github.io/#dataset you can examine them in rviz or in foxglove studio. In this gifs RTK status was in float so not the most accurate but still quite good.
The way we determined orientation was to use the Novatel GPS as a reference (our hypothesis was that it has correct orientation) and we changed Duro orientation accordance that. Later we were able to create 3D pointcloud maps which confirmed our initial hypothesis watch this as a video. So I believe that the orientation is correct, at least for our purposes.

[anshu3012]

Thanks for this @horverno. Great visualisation, helps me draw comparisons between our setups. The setup looks exactly as mine where I have a lowly vectornav 200 instead of Novatel. So the Duro produces the orientation quaternion in the NED frame and the following piece of code converts that NED frame to ENU frame , to match with the orientation of the Novatel or vectornav (And by all means please correct me if I am wrong ) :

   tf2::Quaternion tf_orig(x, y, z, w); 
   tf2::Quaternion tf_rot, tf_aligned; 
   tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation 
   tf_aligned = tf_rot * tf_orig;    // left-handerd / right handed rotation 
   pose_msg.pose.orientation.w = tf_aligned.w() * -1; 
   pose_msg.pose.orientation.x = tf_aligned.y();      // left-handerd / right handed orientation 
   pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation 
   pose_msg.pose.orientation.z = tf_aligned.z(); 

Here is my understanding of what is happening :

tf2::Quaternion tf_orig(x, y, z, w);

Define original quaternion (in NED frame?)

tf_rot.setRPY(0.0, 0.0, -M_PI_2);

Define a quaternion which represents a rotation of -pi/2 in z (in ENU frame?)

tf_aligned = tf_rot * tf_orig;

Rotate our original quaternion by -pi/2 in z (so the tf_aligned is in the ENU frame? What is the need of this rotation?)

   pose_msg.pose.orientation.w = tf_aligned.w() * -1; 
   pose_msg.pose.orientation.x = tf_aligned.y();      // left-handerd / right handed orientation 
   pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation 
   pose_msg.pose.orientation.z = tf_aligned.z(); 

Now this is where it gets confusing. Why are we negating w, swapping x and y and then negating x?