void Selection3DDisplayCustom::transform(Ogre::Vector3& position, Ogre::Quaternion& orientation, const char* from_frame, const char* to_frame)
{
//std::cout << "POS bt: " << position.x << ", " << position.y << ", " << position.z << std::endl;
// put all pose data into a tf stamped pose
tf::Quaternion bt_orientation(orientation.x, orientation.y, orientation.z, orientation.w);
tf::Vector3 bt_position(position.x, position.y, position.z);
std::string frame(from_frame);
tf::Stamped<tf::Pose> pose_in(tf::Transform(bt_orientation,bt_position), ros::Time(), frame);
tf::Stamped<tf::Pose> pose_out;
// convert pose into new frame
try
{
context_->getFrameManager()->getTFClient()->transformPose( to_frame, pose_in, pose_out );
}
catch(tf::TransformException& e)
{
ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", from_frame, to_frame, e.what());
return;
}
bt_position = pose_out.getOrigin();
position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z());
//std::cout << "POS transform: " << position.x << ", " << position.y << ", " << position.z << std::endl;
bt_orientation = pose_out.getRotation();
orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() );
//std::cout << "QUAT transform: " << orientation.x << ", " << orientation.y << ", " << orientation.z << ", " << orientation.w << std::endl;
}
bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, osg::Vec3d& position, osg::Quat& orientation)
{
position.set(0,0,0);
orientation.set(0,0,0,1);
// put all pose data into a tf stamped pose
btQuaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
btVector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
{
bt_orientation.setW(1.0);
}
tf::Stamped<tf::Pose> pose_in(btTransform(bt_orientation,bt_position), time, frame);
tf::Stamped<tf::Pose> pose_out;
// convert pose into new frame
try
{
tf_->transformPose( fixed_frame_, pose_in, pose_out );
}
catch(tf::TransformException& e)
{
ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
return false;
}
#if ROS_VERSION_MINIMUM(1, 8, 0)
//ROS Fuerte version
bt_position = pose_out.asBt().getOrigin();
bt_orientation = pose_out.asBt().getRotation();
#else
bt_position = pose_out.getOrigin();
bt_orientation = pose_out.getRotation();
#endif
position = osg::Vec3d(bt_position.x(), bt_position.y(), bt_position.z());
orientation = osg::Quat( bt_orientation.x(), bt_orientation.y(), bt_orientation.z(), bt_orientation.w() );
return true;
}
bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, Ogre::Vector3& position, Ogre::Quaternion& orientation)
{
if ( !adjustTime(frame, time) )
{
return false;
}
position = Ogre::Vector3::ZERO;
orientation = Ogre::Quaternion::IDENTITY;
// put all pose data into a tf stamped pose
tf::Quaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
tf::Vector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
{
bt_orientation.setW(1.0);
}
tf::Stamped<tf::Pose> pose_in(tf::Transform(bt_orientation,bt_position), time, frame);
tf::Stamped<tf::Pose> pose_out;
// convert pose into new frame
try
{
tf_->transformPose( fixed_frame_, pose_in, pose_out );
}
catch(std::runtime_error& e)
{
ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
return false;
}
bt_position = pose_out.getOrigin();
position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z());
bt_orientation = pose_out.getRotation();
orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() );
return true;
}
