bool FrameManager::getTransform(const std::string& frame, ros::Time time, osg::Vec3d& position, osg::Quat& orientation)
{
    boost::mutex::scoped_lock lock(cache_mutex_);

    position = osg::Vec3d(9999999, 9999999, 9999999);
    orientation.set(0,0,0,1);

    if (fixed_frame_.empty())
    {
        return false;
    }

    M_Cache::iterator it = cache_.find(CacheKey(frame, time));
    if (it != cache_.end())
    {
        position = it->second.position;
        orientation = it->second.orientation;
        return true;
    }

    geometry_msgs::Pose pose;
    pose.orientation.w = 1.0f;

    if (!transform(frame, time, pose, position, orientation))
    {
        return false;
    }

    cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));

    return true;
}
void ComputeOrientation(const osg::Vec3& lv,const osg::Vec3& up, osg::Quat &rotationDest)
{
osg::Vec3 f(lv);
f.normalize();
osg::Vec3 s(f^up);
s.normalize();
osg::Vec3 u(s^f);
u.normalize();


{
__asm NOP
}


osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
                            s[1],     u[1],     -f[1],     0.0f,
                            s[2],     u[2],     -f[2],     0.0f,
                            0.0f,     0.0f,     0.0f,      1.0f);
rotationDest.set(rotation_matrix);
//debug_matrix = rotation_matrix;
//debug_quat = rotationDest;
rotationDest = rotationDest.inverse();

} // ComputeOrientation
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;
}
Exemple #5
0
void osg::av_popMsg(av::Msg& netMsg, ::osg::Quat& buf)
{
    ::osg::Vec4d vec;
    av_popMsg(netMsg, vec);
    buf.set(vec);
}