gazebo_ros_imu.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: 3D position interface for ground truth.
00023  * Author: Sachin Chitta and John Hsu
00024  * Date: 1 June 2008
00025  * SVN info: $Id$
00026  */
00027 
00028 #include <gazebo_plugins/gazebo_ros_imu.h>
00029 
00030 namespace gazebo
00031 {
00032 
00034 // Constructor
00035 GazeboRosIMU::GazeboRosIMU()
00036 {
00037   this->imu_connect_count_ = 0;
00038 }
00039 
00041 // Destructor
00042 GazeboRosIMU::~GazeboRosIMU()
00043 {
00044   event::Events::DisconnectWorldUpdateStart(this->update_connection_);
00045   // Finalize the controller
00046   this->rosnode_->shutdown();
00047   this->callback_queue_thread_.join();
00048   delete this->rosnode_;
00049 }
00050 
00052 // Load the controller
00053 void GazeboRosIMU::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00054 {
00055   // Get the world name.
00056   this->world_ = _parent->GetWorld();
00057 
00058   // load parameters
00059   this->robot_namespace_ = "";
00060   if (_sdf->HasElement("robotNamespace"))
00061     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00062 
00063   if (!_sdf->HasElement("serviceName"))
00064   {
00065     ROS_INFO("imu plugin missing <serviceName>, defaults to /default_imu");
00066     this->service_name_ = "/default_imu";
00067   }
00068   else
00069     this->service_name_ = _sdf->GetElement("serviceName")->GetValueString();
00070 
00071   if (!_sdf->HasElement("topicName"))
00072   {
00073     ROS_INFO("imu plugin missing <topicName>, defaults to /default_imu");
00074     this->topic_name_ = "/default_imu";
00075   }
00076   else
00077     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00078 
00079   if (!_sdf->HasElement("gaussianNoise"))
00080   {
00081     ROS_INFO("imu plugin missing <gaussianNoise>, defaults to 0.0");
00082     this->gaussian_noise_ = 0;
00083   }
00084   else
00085     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00086 
00087   if (!_sdf->HasElement("bodyName"))
00088   {
00089     ROS_FATAL("imu plugin missing <bodyName>, cannot proceed");
00090     return;
00091   }
00092   else
00093     this->link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00094 
00095   if (!_sdf->HasElement("xyzOffset"))
00096   {
00097     ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
00098     this->offset_.pos = math::Vector3(0,0,0);
00099   }
00100   else
00101     this->offset_.pos = _sdf->GetElement("xyzOffset")->GetValueVector3();
00102 
00103   if (!_sdf->HasElement("rpyOffset"))
00104   {
00105     ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
00106     this->offset_.rot = math::Vector3(0,0,0);
00107   }
00108   else
00109     this->offset_.rot = _sdf->GetElement("rpyOffset")->GetValueVector3();
00110 
00111 
00112   // start ros node
00113   if (!ros::isInitialized())
00114   {
00115     int argc = 0;
00116     char** argv = NULL;
00117     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00118   }
00119 
00120   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00121 
00122   // assert that the body by link_name_ exists
00123   this->link = boost::shared_dynamic_cast<physics::Link>(this->world_->GetEntity(this->link_name_));
00124   if (!this->link)
00125   {
00126     ROS_FATAL("gazebo_ros_imu plugin error: bodyName: %s does not exist\n",this->link_name_.c_str());
00127     return;
00128   }
00129 
00130   // if topic name specified as empty, do not publish (then what is this plugin good for?)
00131   if (this->topic_name_ != "")
00132   {
00133     ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::Imu>(
00134       this->topic_name_,1,
00135       boost::bind( &GazeboRosIMU::IMUConnect,this),
00136       boost::bind( &GazeboRosIMU::IMUDisconnect,this), ros::VoidPtr(), &this->imu_queue_);
00137     this->pub_ = this->rosnode_->advertise(ao);
00138 
00139     // advertise services on the custom queue
00140     ros::AdvertiseServiceOptions aso = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00141         this->service_name_,boost::bind( &GazeboRosIMU::ServiceCallback, this, _1, _2 ), ros::VoidPtr(), &this->imu_queue_);
00142     this->srv_ = this->rosnode_->advertiseService(aso);
00143   }
00144 
00145   // Initialize the controller
00146   this->last_time_ = this->world_->GetSimTime();
00147   //this->initial_pose_ = this->link->GetPose(); // get initial pose of the local link
00148   this->last_vpos_ = this->link->GetWorldLinearVel(); // get velocity in gazebo frame
00149   this->last_veul_ = this->link->GetWorldAngularVel(); // get velocity in gazebo frame
00150   this->apos_ = 0;
00151   this->aeul_ = 0;
00152 
00153   // start custom queue for imu
00154   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::IMUQueueThread,this ) );
00155 
00156 
00157   // New Mechanism for Updating every World Cycle
00158   // Listen to the update event. This event is broadcast every
00159   // simulation iteration.
00160   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00161       boost::bind(&GazeboRosIMU::UpdateChild, this));
00162 }
00163 
00165 // returns true always, imu is always calibrated in sim
00166 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
00167                                         std_srvs::Empty::Response &res)
00168 {
00169   return true;
00170 }
00171 
00173 // Increment count
00174 void GazeboRosIMU::IMUConnect()
00175 {
00176   this->imu_connect_count_++;
00177 }
00179 // Decrement count
00180 void GazeboRosIMU::IMUDisconnect()
00181 {
00182   this->imu_connect_count_--;
00183 }
00184 
00186 // Update the controller
00187 void GazeboRosIMU::UpdateChild()
00188 {
00189   if ((this->imu_connect_count_ > 0 && this->topic_name_ != ""))
00190   {
00191     math::Pose pose;
00192     math::Quaternion rot;
00193     math::Vector3 pos;
00194 
00195     // Get Pose/Orientation ///@todo: verify correctness
00196     pose = this->link->GetWorldPose(); // - this->link->GetCoMPose();
00197     // apply xyz offsets and get position and rotation components
00198     pos = pose.pos + this->offset_.pos;
00199     rot = pose.rot;
00200     // std::cout << " --------- GazeboRosIMU rot " << rot.x << ", " << rot.y << ", " << rot.z << ", " << rot.u << std::endl;
00201 
00202     // apply rpy offsets
00203     rot = this->offset_.rot*rot;
00204     rot.Normalize();
00205 
00206     common::Time cur_time = this->world_->GetSimTime();
00207     
00208     // get Rates
00209     math::Vector3 vpos = this->link->GetWorldLinearVel(); // get velocity in gazebo frame
00210     math::Vector3 veul = this->link->GetWorldAngularVel(); // get velocity in gazebo frame
00211 
00212     // differentiate to get accelerations
00213     double tmp_dt = this->last_time_.Double() - cur_time.Double();
00214     if (tmp_dt != 0)
00215     {
00216       this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
00217       this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
00218       this->last_vpos_ = vpos;
00219       this->last_veul_ = veul;
00220     }
00221 
00222     // copy data into pose message
00223     this->imu_msg_.header.frame_id = this->link_name_;
00224     this->imu_msg_.header.stamp.sec = cur_time.sec;
00225     this->imu_msg_.header.stamp.nsec = cur_time.nsec;
00226 
00227     // orientation quaternion
00228 
00229     // uncomment this if we are reporting orientation in the local frame
00230     // not the case for our imu definition
00231     // // apply fixed orientation offsets of initial pose
00232     // rot = this->initial_pose_.rot*rot;
00233     // rot.Normalize();
00234 
00235     this->imu_msg_.orientation.x = rot.x;
00236     this->imu_msg_.orientation.y = rot.y;
00237     this->imu_msg_.orientation.z = rot.z;
00238     this->imu_msg_.orientation.w = rot.w;
00239 
00240     // pass euler angular rates
00241     math::Vector3 linear_velocity(veul.x + this->GaussianKernel(0,this->gaussian_noise_)
00242                            ,veul.y + this->GaussianKernel(0,this->gaussian_noise_)
00243                            ,veul.z + this->GaussianKernel(0,this->gaussian_noise_));
00244     // rotate into local frame
00245     // @todo: deal with offsets!
00246     linear_velocity = rot.RotateVector(linear_velocity);
00247     this->imu_msg_.angular_velocity.x    = linear_velocity.x;
00248     this->imu_msg_.angular_velocity.y    = linear_velocity.y;
00249     this->imu_msg_.angular_velocity.z    = linear_velocity.z;
00250 
00251     // pass accelerations
00252     math::Vector3 linear_acceleration(apos_.x + this->GaussianKernel(0,this->gaussian_noise_)
00253                                      ,apos_.y + this->GaussianKernel(0,this->gaussian_noise_)
00254                                      ,apos_.z + this->GaussianKernel(0,this->gaussian_noise_));
00255     // rotate into local frame
00256     // @todo: deal with offsets!
00257     linear_acceleration = rot.RotateVector(linear_acceleration);
00258     this->imu_msg_.linear_acceleration.x    = linear_acceleration.x;
00259     this->imu_msg_.linear_acceleration.y    = linear_acceleration.y;
00260     this->imu_msg_.linear_acceleration.z    = linear_acceleration.z;
00261 
00262     // fill in covariance matrix
00265     double gn2 = this->gaussian_noise_*this->gaussian_noise_;
00266     this->imu_msg_.orientation_covariance[0] = gn2;
00267     this->imu_msg_.orientation_covariance[4] = gn2;
00268     this->imu_msg_.orientation_covariance[8] = gn2;
00269     this->imu_msg_.angular_velocity_covariance[0] = gn2;
00270     this->imu_msg_.angular_velocity_covariance[4] = gn2;
00271     this->imu_msg_.angular_velocity_covariance[8] = gn2;
00272     this->imu_msg_.linear_acceleration_covariance[0] = gn2;
00273     this->imu_msg_.linear_acceleration_covariance[4] = gn2;
00274     this->imu_msg_.linear_acceleration_covariance[8] = gn2;
00275 
00276     this->lock_.lock();
00277     // publish to ros
00278     if (this->imu_connect_count_ > 0 && this->topic_name_ != "")
00279         this->pub_.publish(this->imu_msg_);
00280     this->lock_.unlock();
00281 
00282     // save last time stamp
00283     this->last_time_ = cur_time;
00284   }
00285 }
00286 
00287 
00289 // Utility for adding noise
00290 double GazeboRosIMU::GaussianKernel(double mu,double sigma)
00291 {
00292   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00293   // see wikipedia
00294   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00295   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00296   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00297   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00298   // we'll just use X
00299   // scale to our mu and sigma
00300   X = sigma * X + mu;
00301   return X;
00302 }
00303 
00305 // Put laser data to the interface
00306 void GazeboRosIMU::IMUQueueThread()
00307 {
00308   static const double timeout = 0.01;
00309 
00310   while (this->rosnode_->ok())
00311   {
00312     this->imu_queue_.callAvailable(ros::WallDuration(timeout));
00313   }
00314 }
00315 
00316 // Register this plugin with the simulator
00317 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
00318 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44