Esempio n. 1
0
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
       );
  }