void BaseApplication::syncCallback(const sensor_msgs::CompressedImageConstPtr& depthImg, const sensor_msgs::CompressedImageConstPtr& rgbImg) {
if (!syncedUpdate) {
Ogre::MemoryDataStream depthStr(depthImg->data.size(), false, false);
Ogre::MemoryDataStream rgbStr(rgbImg->data.size(), false, false);
Ogre::uint8 buffer1, buffer2, buffer3, buffer4;
for (int d=0; d<depthImg->data.size(); d++) {
buffer1 = depthImg->data[d];
buffer2 = depthImg->data[d+1];
buffer3 = depthImg->data[d+2];
buffer4 = depthImg->data[d+3];
if (buffer1 == 137 && buffer2 == 80 && buffer3 == 78 && buffer4 == 71) {
depthStr.write(&depthImg->data[d],depthImg->data.size()-d);
break;
}
}
depthStr.seek(0); //Reset stream position
Ogre::DataStreamPtr *pDstr = new Ogre::DataStreamPtr(&depthStr);
for (int d=0; d<rgbImg->data.size(); d++) {
buffer1 = rgbImg->data[d];
buffer2 = rgbImg->data[d+1];
if (buffer1 == 255 && buffer2 == 216) {
rgbStr.write(&rgbImg->data[d], rgbImg->data.size()-d);
break;
}
}
rgbStr.seek(0); // Reset stream position
Ogre::DataStreamPtr *pRstr = new Ogre::DataStreamPtr(&rgbStr);
using namespace Ogre;
static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
static tfScalar yaw,pitch,roll;
static Matrix3 mRot;
try {
tfListener->lookupTransform("map", "head_xtion_depth_optical_frame", depthImg->header.stamp, snTransform);
snPos.x = snTransform.getOrigin().x();
snPos.y = snTransform.getOrigin().y();
snPos.z = snTransform.getOrigin().z();
snPos = qRot*snPos;
snTransform.getBasis().getEulerYPR(yaw,pitch,roll);
mRot.FromEulerAnglesZYX(Radian(yaw),Radian(pitch),Radian(roll));
snOri.FromRotationMatrix(mRot);
snOri = qRot*snOri;
depImage.load(*pDstr, "png");
texImage.load(*pRstr, "jpeg");
syncedUpdate = true;
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
}
}
void DemoApplication::SmoothCamera::Move (Real deltaTranslation, Real deltaStrafe, Radian pitchAngleStep, Radian yawAngleStep)
{
// here we update the camera movement at simulation rate
m_cameraYawAngle = fmodf (m_cameraYawAngle.valueRadians() + yawAngleStep.valueRadians(), 3.141592f * 2.0f);
m_cameraPitchAngle = Math::Clamp (m_cameraPitchAngle.valueRadians() + pitchAngleStep.valueRadians(), - 80.0f * 3.141592f / 180.0f, 80.0f * 3.141592f / 180.0f);
Matrix3 rot;
rot.FromEulerAnglesZYX (Radian (0.0f), m_cameraYawAngle, m_cameraPitchAngle);
Matrix4 matrix (rot);
m_cameraTranslation += Vector3 (matrix[0][2], matrix[1][2], matrix[2][2]) * deltaTranslation;
m_cameraTranslation += Vector3 (matrix[0][0], matrix[1][0], matrix[2][0]) * deltaStrafe;
matrix.setTrans(m_cameraTranslation);
matrix = matrix.transpose();
Update (matrix[0]);
}
Quaternion OgreMaxUtilities::LoadRotation(const TiXmlElement* objectElement)
{
Quaternion rotation = Quaternion::IDENTITY;
if (objectElement->Attribute("qx") != 0)
{
//The rotation is specified as a quaternion
rotation.x = GetRealAttribute(objectElement, "qx", 0);
rotation.y = GetRealAttribute(objectElement, "qy", 0);
rotation.z = GetRealAttribute(objectElement, "qz", 0);
rotation.w = GetRealAttribute(objectElement, "qw", 0);
}
else if (objectElement->Attribute("axisX") != 0)
{
//The rotation is specified as an axis and angle
Real angle = GetRealAttribute(objectElement, "angle", 0);
Vector3 axis;
axis.x = GetRealAttribute(objectElement, "axisX", 0);
axis.y = GetRealAttribute(objectElement, "axisY", 0);
axis.z = GetRealAttribute(objectElement, "axisZ", 0);
//Convert the angle and axis into the rotation quaternion
rotation.FromAngleAxis(Radian(angle), axis);
}
else if (objectElement->Attribute("angleX") != 0)
{
//Assume the rotation is specified as three Euler angles
Vector3 euler;
euler.x = GetRealAttribute(objectElement, "angleX", 0);
euler.y = GetRealAttribute(objectElement, "angleY", 0);
euler.z = GetRealAttribute(objectElement, "angleZ", 0);
String order = GetStringAttribute(objectElement, "order");
//Convert Euler angles to a matrix
Matrix3 rotationMatrix;
if (order.length() < 2)
rotationMatrix.FromEulerAnglesXYZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
{
if (order[0] == 'x')
{
if (order[1] == 'y')
rotationMatrix.FromEulerAnglesXYZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesXZY(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
else if (order[0] == 'y')
{
if (order[1] == 'x')
rotationMatrix.FromEulerAnglesYXZ(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesYZX(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
else
{
if (order[1] == 'x')
rotationMatrix.FromEulerAnglesZXY(Radian(euler.x), Radian(euler.y), Radian(euler.z));
else
rotationMatrix.FromEulerAnglesZYX(Radian(euler.x), Radian(euler.y), Radian(euler.z));
}
}
//Convert the matrix into the rotation quaternion
rotation.FromRotationMatrix(rotationMatrix);
}
return rotation;
}