bool
SpatialReference::transformFromECEF(const osg::Vec3d& input,
osg::Vec3d& output ) const
{
// transform to lat/long:
osg::Vec3d geo;
getGeographicSRS()->getEllipsoid()->convertXYZToLatLongHeight(
input.x(), input.y(), input.z(),
geo.y(), geo.x(), geo.z() );
// then convert to the local SRS.
if ( isGeographic() )
{
output.set( osg::RadiansToDegrees(geo.x()), osg::RadiansToDegrees(geo.y()), geo.z() );
}
else
{
getGeographicSRS()->transform(
osg::RadiansToDegrees(geo.x()), osg::RadiansToDegrees(geo.y()), geo.z(),
this,
output.x(), output.y(), output.z() );
//output.z() = geo.z();
}
return true;
}
void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
if (!firstpass) {
initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
wMv_initial.setTrans(initialT);
wMv_initial.setRotate(initialQ);
firstpass=true;
}
}
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;
}
// doc in parent
void OrbitCameraManipulator::getTransformation( osg::Vec3d& eye, osg::Vec3d& center, osg::Vec3d& up ) const
{
center.set( m_lookat );
eye.set( m_eye );
up.set( m_up );
}
// doc in parent
void OrbitCameraManipulator::getTransformation( osg::Vec3d& eye, osg::Quat& rotation ) const
{
eye.set( m_eye );
// TODO: implement
}
void UFOManipulator::getCurrentPositionAsLookAt( osg::Vec3d& eye, osg::Vec3d& center, osg::Vec3d& up )
{
eye = _position;
center = _position + _direction;
up.set(getUpVector(getCoordinateFrame(_position)));
}