uwsim: PressureSensor.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/PressureSensor.h>
00015 
00016 double PressureSensor::getMeasurement()
00017 {
00018   //Should get world coords and then transform to the localizedWorld
00019   boost::shared_ptr<osg::Matrix> rMs = getWorldCoords(node_);
00020   osg::Matrixd lMs = *rMs * osg::Matrixd::inverse(rMl_);
00021 
00022   //Now add some gaussian noise
00023   static boost::normal_distribution<> normal(0, std_);
00024   static boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng_, normal);
00025 
00026   return lMs.getTrans().z() + var_nor();
00027 }