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
...
Originally posted by Jägermeister on ROS Answers with karma: 81 on 2019-02-04
Post score: 0
Original comments
Comment by kosmastsk on 2019-02-04:
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
Comment by PeteBlackerThe3rd on 2019-02-04:
It is indeed more complicated than that, in all cases I'm afraid. See my answer below.
Comment by PeteBlackerThe3rd on 2019-02-05:
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.
Comment by Jägermeister on 2019-02-05:
Is the function problematic or the way I use it? I don't get it. Is there a bug in ROS, seriously?
Comment by PeteBlackerThe3rd on 2019-02-05:
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.
Comment by Jägermeister on 2019-02-05:
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.
Comment by PeteBlackerThe3rd on 2019-02-05:
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 vector
Comment by PeteBlackerThe3rd on 2019-02-05:
One of the values your printing out 6.12323399574e-17 is Exactly twice the value in the quaternion of 3.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.
Comment by Jägermeister on 2019-02-06:
I put everything on the ROS side. If you want to see the OpenCV side (how I get the rotation angle out of 2D vision) I can do that too, I doubt it is relevant though.
Comment by PeteBlackerThe3rd on 2019-02-06:
I've just checked and the values coming out of the quaternion_about_axis function are correct, but there must be a bug between there and where your publishing the pose. Can you post all your code between this function and the publish call so we can try and find where it's going wrong.
Comment by Jägermeister on 2019-02-06:
Normally I couldn't do this due to confidential issues but now I found that I can give a time-based link, so perhaps you can look at it, and if you find the problem, we could reshape the question without exposing the code. Here for a short amount of time.
Comment by PeteBlackerThe3rd on 2019-02-06:
There are still no ros publishers in that code. But I think you need to carefully debug through the code because the orientation values are not being copied at some point. The quaternion_about_axis function calculates them correctly but the values are not making it into the pose message you show.
Comment by Jägermeister on 2019-02-06:
I traced the values until the end of the function and they are fine, but then it's all about the ROS service, which returns them as zero. It's really weird, I am about to call it a bug almost. Never seen such a thing before tbh, but I am not the ROS master either. Anyway, thank you for the effort.