uwsim: InertialMeasurementUnit.cpp Source File

Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #include <uwsim/UWSimUtils.h>
00014 #include <uwsim/InertialMeasurementUnit.h>
00015 #include <osg/PositionAttitudeTransform>
00016 
00017 osg::Quat InertialMeasurementUnit::getMeasurement()
00018 {
00019   //Should get world coords and then transform to the localizedWorld
00020   boost::shared_ptr<osg::Matrix> rMi = getWorldCoords(imu_node_);
00021   osg::Matrixd lMi = *rMi * osg::Matrixd::inverse(rMl_);
00022 
00023   //Now add some gaussian noise
00024   static boost::normal_distribution<> normal(0, std_);
00025   static boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng_, normal);
00026 
00027   return lMi.getRotate()
00028       * osg::Quat(var_nor(), osg::Vec3d(1, 0, 0), var_nor(), osg::Vec3d(0, 1, 0), var_nor(), osg::Vec3d(0, 0, 1));
00029 }
00030 
00031 int InertialMeasurementUnit::getTFTransform(tf::Pose & pose, std::string & parent){
00032   parent=parentLinkName;
00033   pose.setOrigin(tf::Vector3(parent_->asTransform()->asPositionAttitudeTransform()->getPosition().x(),
00034                         parent_->asTransform()->asPositionAttitudeTransform()->getPosition().y(),
00035                         parent_->asTransform()->asPositionAttitudeTransform()->getPosition().z()));
00036   pose.setRotation( tf::Quaternion(parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().x(),
00037                         parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().y(),
00038                         parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().z(),
00039                         parent_->asTransform()->asPositionAttitudeTransform()->getAttitude().w()));
00040   return 1;
00041 
00042 }