bool leatherman::quatFromMsg(const geometry_msgs::Quaternion &qmsg, Eigen::Quaterniond &q) { q = Eigen::Quaterniond(qmsg.w, qmsg.x, qmsg.y, qmsg.z); if (fabs(q.squaredNorm() - 1.0) > 1e-3) { q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); return false; } return true; }
bool planning_models::quatFromMsg(const geometry_msgs::Quaternion &qmsg, Eigen::Quaterniond &q) { q = Eigen::Quaterniond(qmsg.w, qmsg.x, qmsg.y, qmsg.z); double error = fabs(q.squaredNorm() - 1.0); if (error > 0.05) { ROS_ERROR("Quaternion is NOWHERE CLOSE TO NORMALIZED [x,y,z,w], [%.2f, %.2f, %.2f, %.2f]. Can't do much, returning identity.", qmsg.x, qmsg.y, qmsg.z, qmsg.w); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); return false; } else if(error > 1e-3) q.normalize(); return true; }