void SceneNodeProvider::setOffsets(const Ogre::Vector3& translate, const Ogre::Quaternion& rotate) {
if (translate.isNaN() || rotate.isNaN()
|| (translate == Ogre::Vector3::ZERO && (rotate == Ogre::Quaternion::IDENTITY || rotate == Ogre::Quaternion::ZERO))) {
if (mOffsetNode) {
while (mOffsetNode->numAttachedObjects()) {
auto movable = mOffsetNode->detachObject((unsigned short) 0);
mNode->attachObject(movable);
}
}
} else {
if (mOffsetNode) {
mOffsetNode->setPosition(translate);
mOffsetNode->setOrientation(rotate);
} else {
mOffsetNode = mNode->createChildSceneNode(translate, rotate);
mOffsetNode->setInheritScale(true);
while (mNode->numAttachedObjects()) {
auto movable = mNode->detachObject((unsigned short) 0);
mOffsetNode->attachObject(movable);
}
}
}
}
void
CovarianceVisual::setMessage
(const geometry_msgs::PoseWithCovariance& msg)
{
// Construct pose position and orientation.
const geometry_msgs::Point& p = msg.pose.position;
Ogre::Vector3 position (p.x, p.y, p.z);
Ogre::Quaternion orientation
(msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z);
// Set position and orientation for axes scene node.
if(!position.isNaN())
axes_->setPosition (position);
else
ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position);
if(!orientation.isNaN())
axes_->setOrientation (orientation);
else
ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation);
// check for NaN in covariance
for (unsigned i = 0; i < 3; ++i) {
if(isnan(msg.covariance[i])) {
ROS_WARN_THROTTLE(1, "covariance contains NaN");
return;
}
}
// Compute eigen values and vectors for both shapes.
std::pair<Eigen::Matrix3d, Eigen::Vector3d>
positionEigenVectorsAndValues
(computeEigenValuesAndVectors (msg, 0));
std::pair<Eigen::Matrix3d, Eigen::Vector3d>
orientationEigenVectorsAndValues
(computeEigenValuesAndVectors (msg, 3));
Ogre::Quaternion positionQuaternion
(computeRotation (msg, positionEigenVectorsAndValues));
Ogre::Quaternion orientationQuaternion
(computeRotation (msg, orientationEigenVectorsAndValues));
positionNode_->setOrientation (positionQuaternion);
orientationNode_->setOrientation (orientationQuaternion);
// Compute scaling.
Ogre::Vector3 positionScaling
(std::sqrt (positionEigenVectorsAndValues.second[0]),
std::sqrt (positionEigenVectorsAndValues.second[1]),
std::sqrt (positionEigenVectorsAndValues.second[2]));
positionScaling *= scaleFactor_;
Ogre::Vector3 orientationScaling
(std::sqrt (orientationEigenVectorsAndValues.second[0]),
std::sqrt (orientationEigenVectorsAndValues.second[1]),
std::sqrt (orientationEigenVectorsAndValues.second[2]));
orientationScaling *= scaleFactor_;
// Set the scaling.
if(!positionScaling.isNaN())
positionNode_->setScale (positionScaling);
else
ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling);
if(!orientationScaling.isNaN())
orientationNode_->setScale (orientationScaling);
else
ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling);
// Debugging.
ROS_DEBUG_STREAM_THROTTLE
(1.,
"Position:\n"
<< position << "\n"
<< "Positional part 3x3 eigen values:\n"
<< positionEigenVectorsAndValues.second << "\n"
<< "Positional part 3x3 eigen vectors:\n"
<< positionEigenVectorsAndValues.first << "\n"
<< "Sphere orientation:\n"
<< positionQuaternion << "\n"
<< positionQuaternion.getRoll () << " "
<< positionQuaternion.getPitch () << " "
<< positionQuaternion.getYaw () << "\n"
<< "Sphere scaling:\n"
<< positionScaling << "\n"
<< "Rotational part 3x3 eigen values:\n"
<< orientationEigenVectorsAndValues.second << "\n"
<< "Rotational part 3x3 eigen vectors:\n"
<< orientationEigenVectorsAndValues.first << "\n"
<< "Cone orientation:\n"
<< orientationQuaternion << "\n"
<< orientationQuaternion.getRoll () << " "
<< orientationQuaternion.getPitch () << " "
<< orientationQuaternion.getYaw () << "\n"
<< "Cone scaling:\n"
<< orientationScaling
);
}
