Rotation Angle in Pose Orientation
I have a 2D image on which I conduct an algorithm to find its rotation, and I get it in radian. No problem until here.
Now that I want to fill in the pose object with what I collected from the 2D vision, I get stuck at where to insert the rotation. I do know I cannot fill in everything, I don't intend to, either. I only need the position (I have it already) and the rotation.
obj.pose.position.x = v['pose'][0] #x
obj.pose.position.y = v['pose'][1] #y
obj.pose.position.z = v['pose'][2] #z
# which one below is the one I should use to pass the rotation angle?
obj.pose.orientation.x = v['pose'][3]
obj.pose.orientation.y = v['pose'][4]
obj.pose.orientation.z = v['pose'][5]
obj.pose.orientation.w = v['pose'][6]
Any thoughts?
EDIT: So I came up with the following after the useful insights given by the commentators.
self.some_srv = rospy.Service("/bla/request_poses", PartArray, self.some_service_srv)
def some_service_srv(self, req):
# some irrelevant stuff happening here
# this is where the actual thing shall happen
obj.pose.position.x = v['pose'][0]
obj.pose.position.y = v['pose'][1]
obj.pose.position.z = v['pose'][2]
rotation_angle = v['pose'][3] # extract the rotation angle
zaxis = (0, 0, 1) #rotation is around the Z axis in the image
quaternion = quaternion_about_axis(rotation_angle, zaxis) # use rotation angle + axis to get the quaternion
obj.pose.orientation.x = quaternion[0]
obj.pose.orientation.y = quaternion[1]
obj.pose.orientation.z = quaternion[2] # this is the rotation we are talking about
obj.pose.orientation.w = quaternion[3]
list_detected.part_array.append(obj)
return list_detected
This runs, however I always get 0 values for all of the quarternions, which is somehow nonsense because the rotation angle is definitely non-zero (I print its value).
position:
x: 274.0
y: 250.0
z: 602.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
I print the quarternion list separately, and there is a non-zero value inside, which seems fine, but I don't know why the published value gets zero.
[0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00]
I also printed the value of rotation (in radian) and here are some values of it:
0.980580687945
6.12323399574e-17
-0.707106781187
0.294085862655
-1.0
0.56419054038
-0.0114934175734
6.12323399574e-17
6.12323399574e-17
-1.0
0.707106781187
6.12323399574e-17
And this is how I get the angle using OpenCV:
# helper function to find out which side of the rectangle is longer, then add the angle appropriately
def getAngle(rect):
angle = rect[2]
width, height = rect[1]
if (width < height):
return math.cos(math.radians(angle + 180))
else:
return math.cos(math.radians(angle + 90))
...
# Find contours:
(im, contours, hierarchy) = cv2.findContours(im, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
cnt = contours[0]
# get the rotated rectangle to get the rotation
rect = cv2.minAreaRect(cnt)
rotation = getAngle(rect) # in radians
...
Considering that the image is on the xy plane, I would say that the rotation is on the orientation.z. Think of it, as turning the image around the z axis, but keeping the other axes stable. If the image is not on one axis, it may be more complicated though
It is indeed more complicated than that, in all cases I'm afraid. See my answer below.
Even if your rotation was zero the quaternion would contain positive values. The quaternion values you printed out is very small (x10-17) so is being rounded to zero. There appears to be a problem with the
quaternion_about_axis
function since it's returning an invalid rotation quaternion.Is the function problematic or the way I use it? I don't get it. Is there a bug in ROS, seriously?
The function definitely works, can you show us more of your code. It's probably something about how your getting the values into the message.
I edited my question, butt there isn't much to write really, I mean, I wrote all that is relevant. It's just a service which is called, and it should return the pose (position + quarternion). Position values are filled in, no problem, but the quaternion values are always zero for some reason.
Can you print the value and type of
v['pose'][3]
is this not numeric then it may explain this behaviour. I've just checked the syntax and I think you're doing anything obvious wrong. The function really should be returning a 4D unit vectorOne of the values your printing out
6.12323399574e-17
is Exactly twice the value in the quaternion of3.061617e-17
I suspect there is some other code you haven't put in your question which is causing the problem. Can you post all your code which is producing the problem.