Ask Your Question

How to calculate quaterion for 2d navigation between 2 points

asked 2019-06-06 19:28:09 -0600

Usui gravatar image

updated 2019-06-10 12:17:06 -0600

So I was doing 2d navigation and I am basically doing using this code:

In there it has:

goal.target_pose.pose.position.x = 1;

goal.target_pose.pose.orientation.w = 1;

and this is suppose to make it go straight. I am guess it is at (0,0) and it is going to (1,0). So I am wondering how I can calculate quaternion between 2 points if I want it to go to a point. And if that's the concept. Because as I see it, the line orientation is rotating at a certain angle to be able to go to the given point (x,y). So it's rotate at orientation.w then go straight to (position.x, position.y). If that's not it it's work please tell me if it is also please tell me.

Also I did a little digging and found this:

float x[3] = {1.228,2.57,0.014};
float y[3] = {-3.34,-0.97,-0.97};
float w[3] = {-0.48,0.33,-2.58};

goal.target_pose.pose.position.x = x[i];
goal.target_pose.pose.position.y = y[i];
goal.target_pose.pose.orientation.w = w[i];

It is sending goal to 3 points and it keeps going. But I don't know how they calculate the w[3] array.

Also I found this:

double radians = theta * (M_PI/180);
tf::Quaternion quaternion;
quaternion = tf::createQuaternionFromYaw(radians);
geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(quaternion, qMsg);
goal.target_pose.pose.orientation = qMsg;

and I am wondering if it is right and if it is elaborate a little bit on it.

Any help is appreciated! Thank you!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2019-06-07 04:36:09 -0600

The first thing I would suggest is reading up a little bit about quaternions here. They are 4 dimensional unit vectors, so setting w=1 is only a valid quaternion in a very specific case.

Generating a rotation quaternion isn't the simplest thing in the world which is why several functions to do this are built into the ROS tf library. The tf::createQuaternionFromYaw function you've found is one of these and generates a valid rotation quaternion from a rotation about the Z axis.

To use this function you'll need to know the angle necessary to get to the goal point. For this you'll need to calculate the relative vector between the start and goal points then use the atan2(x,y) function to calculate the required angle.

Hope this helps.

edit flag offensive delete link more


What is atan2(x,y)? Is the atan2(y,x)?

So let's say my current pose is (1.228, -3.34) and the goal is (2.57, -0.97) the angle would be atan2(1.342, 2.37)? and I would convert that to quaternion using the code that I had up there?

Usui gravatar image Usui  ( 2019-06-07 07:06:10 -0600 )edit

Yes that's it, you would use the createQuaternionFromYaw function to convert the angle from atan2 into a quaternion.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-06-07 10:12:52 -0600 )edit

@PeteBlackerThe3rd Okay so I did what you said but when I test it out in the move_base using sendgoal it is not working. Here is the code:

Is it because of my code or something else? Thank you!

Usui gravatar image Usui  ( 2019-06-10 12:22:28 -0600 )edit

When you say 'it is not working' what exactly do you mean. Failing to compile? Crashing at runtime? Behaving differently to what you expect?

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-06-10 12:28:01 -0600 )edit

It didn't move and gave me a trajectory error

Usui gravatar image Usui  ( 2019-06-10 12:35:25 -0600 )edit

That sounds as though that planner has failed to find a safe route to the goal. Can you copy and paste the actual error you're getting.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-06-10 12:40:41 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2019-06-06 19:28:09 -0600

Seen: 642 times

Last updated: Jun 10 '19