gazebo_ros_p3d.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_p3d.h>
00029 
00030 #include "tf/tf.h"
00031 
00032 namespace gazebo
00033 {
00034 
00035 
00037 // Constructor
00038 GazeboRosP3D::GazeboRosP3D()
00039 {
00040   this->p3d_connect_count_ = 0;
00041 }
00042 
00044 // Destructor
00045 GazeboRosP3D::~GazeboRosP3D()
00046 {
00047   event::Events::DisconnectWorldUpdateStart(this->update_connection_);
00048   // Finalize the controller
00049   this->rosnode_->shutdown();
00050   this->p3d_queue_.clear();
00051   this->p3d_queue_.disable();
00052   this->callback_queue_thread_.join();
00053   delete this->rosnode_;
00054 }
00055 
00057 // Load the controller
00058 void GazeboRosP3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00059 {
00060   // Get the world name.
00061   this->world_ = _parent->GetWorld();
00062   this->model_ = _parent;
00063 
00064   // load parameters
00065   this->robot_namespace_ = "";
00066   if (_sdf->HasElement("robotNamespace"))
00067     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00068 
00069   if (!_sdf->HasElement("bodyName"))
00070   {
00071     ROS_FATAL("p3d plugin missing <bodyName>, cannot proceed");
00072     return;
00073   }
00074   else
00075     this->link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00076 
00077   this->link_ = _parent->GetLink(this->link_name_);
00078   if (!this->link_)
00079   {
00080     ROS_FATAL("gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",this->link_name_.c_str());
00081     return;
00082   }
00083 
00084   if (!_sdf->HasElement("topicName"))
00085   {
00086     ROS_FATAL("p3d plugin missing <topicName>, cannot proceed");
00087     return;
00088   }
00089   else
00090     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00091 
00092   if (!_sdf->HasElement("frameName"))
00093   {
00094     ROS_INFO("p3d plugin missing <frameName>, defaults to world");
00095     this->frame_name_ = "world";
00096   }
00097   else
00098     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00099 
00100   if (!_sdf->HasElement("xyzOffset"))
00101   {
00102     ROS_INFO("p3d plugin missing <xyzOffset>, defaults to 0s");
00103     this->offset_.pos = math::Vector3(0,0,0);
00104   }
00105   else
00106     this->offset_.pos = _sdf->GetElement("xyzOffset")->GetValueVector3();
00107 
00108   if (!_sdf->HasElement("rpyOffset"))
00109   {
00110     ROS_INFO("p3d plugin missing <rpyOffset>, defaults to 0s");
00111     this->offset_.rot = math::Vector3(0,0,0);
00112   }
00113   else
00114     this->offset_.rot = _sdf->GetElement("rpyOffset")->GetValueVector3();
00115 
00116   if (!_sdf->HasElement("gaussianNoise"))
00117   {
00118     ROS_INFO("p3d plugin missing <gaussianNoise>, defaults to 0.0");
00119     this->gaussian_noise_ = 0;
00120   }
00121   else
00122     this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->GetValueDouble();
00123 
00124   if (!_sdf->HasElement("updateRate"))
00125   {
00126     ROS_INFO("p3d plugin missing <updateRate>, defaults to 0.0 (as fast as possible)");
00127     this->update_rate_ = 0;
00128   }
00129   else
00130     this->update_rate_ = _sdf->GetElement("updateRate")->GetValueDouble();
00131 
00132   if (!ros::isInitialized())
00133   {
00134     int argc = 0;
00135     char** argv = NULL;
00136     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00137   }
00138 
00139   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00140 
00141   // resolve tf prefix
00142   std::string prefix;
00143   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00144   this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);
00145 
00146   if (this->topic_name_ != "")
00147   {
00148     ros::AdvertiseOptions p3d_ao = ros::AdvertiseOptions::create<nav_msgs::Odometry>(
00149     this->topic_name_,1,
00150     boost::bind( &GazeboRosP3D::P3DConnect,this),
00151     boost::bind( &GazeboRosP3D::P3DDisconnect,this), ros::VoidPtr(), &this->p3d_queue_);
00152     this->pub_ = this->rosnode_->advertise(p3d_ao);
00153   }
00154   
00155   this->last_time_ = this->world_->GetSimTime();
00156   // initialize body
00157   this->last_vpos_ = this->link_->GetWorldLinearVel(); // get velocity in gazebo frame
00158   this->last_veul_ = this->link_->GetWorldAngularVel(); // get velocity in gazebo frame
00159   this->apos_ = 0;
00160   this->aeul_ = 0;
00161 
00163   if (this->frame_name_ != "world" && this->frame_name_ != "/map" && this->frame_name_ != "map")
00164   {
00165     this->reference_link_ = this->model_->GetLink(this->frame_name_);
00166     if (!this->reference_link_)
00167     {
00168       ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will not publish pose\n",this->frame_name_.c_str());
00169       return;
00170     }
00171   }
00172 
00173   // init reference frame state
00174   if (this->reference_link_)
00175   {
00176     ROS_DEBUG("got body %s",this->reference_link_->GetName().c_str());
00177     this->frame_apos_ = 0;
00178     this->frame_aeul_ = 0;
00179     this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel(); // get velocity in gazebo frame
00180     this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel(); // get velocity in gazebo frame
00181   }
00182 
00183 
00184   // start custom queue for p3d
00185   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosP3D::P3DQueueThread,this ) );
00186   
00187   // New Mechanism for Updating every World Cycle
00188   // Listen to the update event. This event is broadcast every
00189   // simulation iteration.
00190   this->update_connection_ = event::Events::ConnectWorldUpdateStart(
00191       boost::bind(&GazeboRosP3D::UpdateChild, this));
00192 }
00193 
00195 // Increment count
00196 void GazeboRosP3D::P3DConnect()
00197 {
00198   this->p3d_connect_count_++;
00199 }
00201 // Decrement count
00202 void GazeboRosP3D::P3DDisconnect()
00203 {
00204   this->p3d_connect_count_--;
00205 }
00206 
00208 // Update the controller
00209 void GazeboRosP3D::UpdateChild()
00210 {
00211   if (!this->link_)
00212     return;
00213 
00214   common::Time cur_time = this->world_->GetSimTime();
00215 
00216   // rate control
00217   if (this->update_rate_ > 0 && (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
00218     return;
00219 
00220   if (this->p3d_connect_count_ > 0)
00221   {
00222 
00223 
00224     // differentiate to get accelerations
00225     double tmp_dt = cur_time.Double() - this->last_time_.Double();
00226     if (tmp_dt != 0)
00227     {
00228       this->lock.lock();
00229 
00230       if (this->topic_name_ != "")
00231       {
00232         // copy data into pose message
00233         this->pose_msg_.header.frame_id = this->tf_frame_name_;  // @todo: should this be changeable?
00234         this->pose_msg_.header.stamp.sec = cur_time.sec;
00235         this->pose_msg_.header.stamp.nsec = cur_time.nsec;
00236 
00237 
00238         math::Pose pose, frame_pose;
00239         math::Vector3 frame_vpos;
00240         math::Vector3 frame_veul;
00241 
00242         // get inertial Rates
00243         math::Vector3 vpos = this->link_->GetWorldLinearVel(); // get velocity in gazebo frame
00244         math::Vector3 veul = this->link_->GetWorldAngularVel(); // get velocity in gazebo frame
00245 
00246         // Get Pose/Orientation
00247         pose = this->link_->GetWorldPose(); // - this->link_->GetCoMPose();
00248 
00249         // Apply Reference Frame
00250         if (this->reference_link_)
00251         {
00252           // convert to relative pose
00253           frame_pose = this->reference_link_->GetWorldPose(); // - this->link_->GetCoMPose();
00254           pose.pos = pose.pos - frame_pose.pos;
00255           pose.pos = frame_pose.rot.RotateVectorReverse(pose.pos);
00256           pose.rot *= frame_pose.rot.GetInverse();
00257           // convert to relative rates
00258           frame_vpos = this->reference_link_->GetWorldLinearVel(); // get velocity in gazebo frame
00259           frame_veul = this->reference_link_->GetWorldAngularVel(); // get velocity in gazebo frame
00260           vpos = frame_pose.rot.RotateVector(vpos - frame_vpos);
00261           veul = frame_pose.rot.RotateVector(veul - frame_veul);
00262         }
00263 
00264         // Apply Constant Offsets
00265         // apply xyz offsets and get position and rotation components
00266         pose.pos = pose.pos + this->offset_.pos;
00267         // apply rpy offsets
00268         pose.rot = this->offset_.rot*pose.rot;
00269         pose.rot.Normalize();
00270 
00271         // compute accelerations (not used)
00272         this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
00273         this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
00274         this->last_vpos_ = vpos;
00275         this->last_veul_ = veul;
00276 
00277         this->frame_apos_ = (this->last_frame_vpos_ - frame_vpos) / tmp_dt;
00278         this->frame_aeul_ = (this->last_frame_veul_ - frame_veul) / tmp_dt;
00279         this->last_frame_vpos_ = frame_vpos;
00280         this->last_frame_veul_ = frame_veul;
00281 
00282         // Fill out messages
00283         this->pose_msg_.pose.pose.position.x    = pose.pos.x;
00284         this->pose_msg_.pose.pose.position.y    = pose.pos.y;
00285         this->pose_msg_.pose.pose.position.z    = pose.pos.z;
00286 
00287         this->pose_msg_.pose.pose.orientation.x = pose.rot.x;
00288         this->pose_msg_.pose.pose.orientation.y = pose.rot.y;
00289         this->pose_msg_.pose.pose.orientation.z = pose.rot.z;
00290         this->pose_msg_.pose.pose.orientation.w = pose.rot.w;
00291 
00292         this->pose_msg_.twist.twist.linear.x  = vpos.x + this->GaussianKernel(0,this->gaussian_noise_) ;
00293         this->pose_msg_.twist.twist.linear.y  = vpos.y + this->GaussianKernel(0,this->gaussian_noise_) ;
00294         this->pose_msg_.twist.twist.linear.z  = vpos.z + this->GaussianKernel(0,this->gaussian_noise_) ;
00295         // pass euler angular rates
00296         this->pose_msg_.twist.twist.angular.x = veul.x + this->GaussianKernel(0,this->gaussian_noise_) ;
00297         this->pose_msg_.twist.twist.angular.y = veul.y + this->GaussianKernel(0,this->gaussian_noise_) ;
00298         this->pose_msg_.twist.twist.angular.z = veul.z + this->GaussianKernel(0,this->gaussian_noise_) ;
00299 
00300         // fill in covariance matrix
00302         double gn2 = this->gaussian_noise_*this->gaussian_noise_;
00303         this->pose_msg_.pose.covariance[0] = gn2;
00304         this->pose_msg_.pose.covariance[7] = gn2;
00305         this->pose_msg_.pose.covariance[14] = gn2;
00306         this->pose_msg_.pose.covariance[21] = gn2;
00307         this->pose_msg_.pose.covariance[28] = gn2;
00308         this->pose_msg_.pose.covariance[35] = gn2;
00309 
00310         this->pose_msg_.twist.covariance[0] = gn2;
00311         this->pose_msg_.twist.covariance[7] = gn2;
00312         this->pose_msg_.twist.covariance[14] = gn2;
00313         this->pose_msg_.twist.covariance[21] = gn2;
00314         this->pose_msg_.twist.covariance[28] = gn2;
00315         this->pose_msg_.twist.covariance[35] = gn2;
00316 
00317         // publish to ros
00318         this->pub_.publish(this->pose_msg_);
00319       }
00320 
00321       this->lock.unlock();
00322 
00323       // save last time stamp
00324       this->last_time_ = cur_time;
00325     }
00326   }
00327 }
00328 
00330 // Utility for adding noise
00331 double GazeboRosP3D::GaussianKernel(double mu,double sigma)
00332 {
00333   // using Box-Muller transform to generate two independent standard normally disbributed normal variables
00334   // see wikipedia
00335   double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00336   double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
00337   double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
00338   //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
00339   // we'll just use X
00340   // scale to our mu and sigma
00341   X = sigma * X + mu;
00342   return X;
00343 }
00344 
00346 // Put laser data to the interface
00347 void GazeboRosP3D::P3DQueueThread()
00348 {
00349   static const double timeout = 0.01;
00350 
00351   while (this->rosnode_->ok())
00352   {
00353     this->p3d_queue_.callAvailable(ros::WallDuration(timeout));
00354   }
00355 }
00356 
00357 GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D);
00358 
00359 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sun Jan 5 2014 11:34:58