interactive markers not fully pitching
Maybe it's my lack of a fundamental understanding of quaternions but I can't seem to pitch an interactive marker beyond 1.57rad. I'm building my own robot model using interactive markers but have also run through the tutorial on basic control of interactive markers and none of them seem to want to pitch past PI/2.
Is this a bug or am I missing something in the mathematics of a quaternion calculation? I'm using tf to convert from a quaternion to euler.
Edit 1: I'm running Fuerte on Precise. I don't generate the quaternion, rather, I receive the pose via callbacks that are registered to the interactive markers which I then convert using tf:
processInteractiveObjectFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
tf::Quaternion q;
tf::quaternionMsgToTF(feedback->pose.orientation, q);
double r, p, y;
tf::Matrix3x3(q).getRPY(r, p, y);
}
The 'Y' value of the quaternion seems to be valid, so I must be missing something with the math behind the 'W' (orientation) value. Basically when I pitch the interactive marker beyond 1.57rad, the pitch starts going back down to 0 as if the quaternion to euler conversion doesn't utilize the orientation?
Edit 2: The following are some images captured and smashed together, overlook the bad quality. I'm using the green ring to pitch the interactive marker down.
Image 1: pitching hand to show result is as expected. example case.
Image 2: pitched hand ~1.57rad. this seems to be the limit for correct pitch values. Y and W from the quaternion still look good.
Image 3: shows the hand (clearly) pitched beyond 1.57rad but the pitch value seems to go back to 0. Y from the quaternion still seems valid (range is +/- 1).
Unfortunately, your question does not provide enough information to help. How is your setup? How are you generating the quaternion? How are you converting it? Which ros distro are you using? Code snippets would also help. Have a look at http://ros.org/wiki/Support
To me, your code looks good. Can you also add the quaternion and the values of rpy for a case that is failing? Maybe also a screenshot of the interactive marker?