/** Checks if the quaternion is a valid navigation goal, i.e. has non-zero
* length and is close to vertical. */
bool isQuaternionValid(const geometry_msgs::Quaternion& q)
{
// Ensure that the quaternion does not have NaN's or infinities
if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
{
ROS_WARN("Quaternion has NaN's or infinities.");
return false;
}
// Ensure that the length of the quaternion is not close to zero
tf::Quaternion tf_q(q.x, q.y, q.z, q.w);
if (tf_q.length2() < 1e-6)
{
ROS_WARN("Quaternion has length close to zero.");
return false;
}
// Normalize the quaternion and check that it transforms the vertical
// vector correctly
tf_q.normalize();
tf::Vector3 up(0, 0, 1);
double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
if (fabs(dot - 1) > 1e-3)
{
ROS_WARN("The z-axis of the quaternion is not close to vertical.");
return false;
}
return true;
}
bool upo_RRT_ros::ValidityChecker2::isQuaternionValid(const geometry_msgs::Quaternion q) {
//first we need to check if the quaternion has nan's or infs
if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
ROS_ERROR("Quaternion has infs!!!!");
return false;
}
if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w)) {
ROS_ERROR("Quaternion has nans !!!");
return false;
}
if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01) {
ROS_ERROR("Quaternion malformed, magnitude: %.3f should be 1.0", (q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w));
return false;
}
tf::Quaternion tf_q(q.x, q.y, q.z, q.w);
//next, we need to check if the length of the quaternion is close to zero
if(tf_q.length2() < 1e-6){
ROS_ERROR("Quaternion has length close to zero... discarding.");
return false;
}
//next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
tf_q.normalize();
tf::Vector3 up(0, 0, 1);
double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
if(fabs(dot - 1) > 1e-3){
ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
return false;
}
return true;
}
// computes a stamped transform for each user joint
geometry_msgs::TransformStamped
getUserTransform(XnUserID const& user, XnSkeletonJoint joint, std::string const& frame_id, std::string const& child_frame_id)
{
XnSkeletonJointPosition joint_position;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
double x = -joint_position.position.X / 1000.0;
double y = joint_position.position.Y / 1000.0;
double z = joint_position.position.Z / 1000.0;
XnSkeletonJointOrientation joint_orientation;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
XnFloat* m = joint_orientation.orientation.elements;
btMatrix3x3 mat (m[0], m[1], m[2],
m[3], m[4], m[5],
m[6], m[7], m[8]);
btQuaternion q;
mat.getRotation (q);
q.setY(-q.y());
q.setZ(-q.z());
tf::Transform transform;
transform.setOrigin(tf::Vector3(x, y, z));
tf::Quaternion tf_q(q.x(), q.y(), q.z(), q.w());
transform.setRotation(tf_q);
geometry_msgs::TransformStamped msg;
// see openni_tracker ticket #4994
tf::Transform change_frame;
change_frame.setOrigin(tf::Vector3(0, 0, 0));
tf::Quaternion frame_rotation;
frame_rotation.setRPY(1.5708, 0, 1.5708);
change_frame.setRotation(frame_rotation);
transform = change_frame * transform;
transformStampedTFToMsg (tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_id), msg);
return msg;
}
