gazebo_ros_imu.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 /*
00018  * Desc: 3D position interface for ground truth.
00019  * Author: Sachin Chitta and John Hsu
00020  * Date: 1 June 2008
00021  */
00022 
00023 #include <gazebo_plugins/gazebo_ros_imu.h>
00024 
00025 namespace gazebo
00026 {
00027 // Register this plugin with the simulator
00028 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
00029 
00030 
00031 // Constructor
00032 GazeboRosIMU::GazeboRosIMU()
00033 {
00034   this->seed = 0;
00035 }
00036 
00038 // Destructor
00039 GazeboRosIMU::~GazeboRosIMU()
00040 {
00041   event::Events::DisconnectWorldUpdateBegin(this->update_connection_);
00042   // Finalize the controller
00043   this->rosnode_->shutdown();
00044   this->callback_queue_thread_.join();
00045   delete this->rosnode_;
00046 }
00047 
00049 // Load the controller
00050 void GazeboRosIMU::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00051 {
00052   // save pointers
00053   this->world_ = _parent->GetWorld();
00054   this->sdf = _sdf;
00055 
00056   // ros callback queue for processing subscription
00057   this->deferred_load_thread_ = boost::thread(
00058     boost::bind(&GazeboRosIMU::LoadThread, this));
00059 }
00060 
00062 // Load the controller
00063 void GazeboRosIMU::LoadThread()
00064 {
00065   // load parameters
00066   this->robot_namespace_ = "";
00067   if (this->sdf->HasElement("robotNamespace"))
00068     this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
00069 
00070   if (!this->sdf->HasElement("serviceName"))
00071   {
00072     ROS_INFO("imu plugin missing <serviceName>, defaults to /default_imu");
00073     this->service_name_ = "/default_imu";
00074   }
00075   else
00076     this->service_name_ = this->sdf->Get<std::string>("serviceName");
00077 
00078   if (!this->sdf->HasElement("topicName"))
00079   {
00080     ROS_INFO("imu plugin missing <topicName>, defaults to /default_imu");
00081     this->topic_name_ = "/default_imu";
00082   }
00083   else
00084     this->topic_name_ = this->sdf->Get<std::string>("topicName");
00085 
00086   if (!this->sdf->HasElement("gaussianNoise"))
00087   {
00088     ROS_INFO("imu plugin missing <gaussianNoise>, defaults to 0.0");
00089     this->gaussian_noise_ = 0.0;
00090   }
00091   else
00092     this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
00093 
00094   if (!this->sdf->HasElement("bodyName"))
00095   {
00096     ROS_FATAL("imu plugin missing <bodyName>, cannot proceed");
00097     return;
00098   }
00099   else
00100     this->link_name_ = this->sdf->Get<std::string>("bodyName");
00101 
00102   if (!this->sdf->HasElement("xyzOffset"))
00103   {
00104     ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
00105     this->offset_.pos = math::Vector3(0, 0, 0);
00106   }
00107   else
00108     this->offset_.pos = this->sdf->Get<math::Vector3>("xyzOffset");
00109 
00110   if (!this->sdf->HasElement("rpyOffset"))
00111   {
00112     ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
00113     this->offset_.rot = math::Vector3(0, 0, 0);
00114   }
00115   else
00116     this->offset_.rot = this->sdf->Get<math::Vector3>("rpyOffset");
00117   
00118   if (!this->sdf->HasElement("updateRate"))
00119   {
00120     ROS_DEBUG("imu plugin missing <updateRate>, defaults to 0.0"
00121              " (as fast as possible)");
00122     this->update_rate_ = 0.0;
00123   }
00124   else
00125     this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();
00126 
00127   if (!this->sdf->HasElement("frameName"))
00128   {
00129     ROS_INFO("imu plugin missing <frameName>, defaults to <bodyName>");
00130     this->frame_name_ = link_name_;
00131   }
00132   else
00133     this->frame_name_ = this->sdf->Get<std::string>("frameName");
00134 
00135   // Make sure the ROS node for Gazebo has already been initialized
00136   if (!ros::isInitialized())
00137   {
00138     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00139       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00140     return;
00141   }
00142 
00143   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00144 
00145   // publish multi queue
00146   this->pmq.startServiceThread();
00147 
00148   // assert that the body by link_name_ exists
00149   this->link = boost::dynamic_pointer_cast<physics::Link>(
00150     this->world_->GetEntity(this->link_name_));
00151   if (!this->link)
00152   {
00153     ROS_FATAL("gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
00154       this->link_name_.c_str());
00155     return;
00156   }
00157 
00158   // if topic name specified as empty, do not publish
00159   if (this->topic_name_ != "")
00160   {
00161     this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
00162     this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
00163       this->topic_name_, 1);
00164 
00165     // advertise services on the custom queue
00166     ros::AdvertiseServiceOptions aso =
00167       ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00168       this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
00169       this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
00170     this->srv_ = this->rosnode_->advertiseService(aso);
00171   }
00172 
00173   // Initialize the controller
00174   this->last_time_ = this->world_->GetSimTime();
00175 
00176   // this->initial_pose_ = this->link->GetPose();
00177   this->last_vpos_ = this->link->GetWorldLinearVel();
00178   this->last_veul_ = this->link->GetWorldAngularVel();
00179   this->apos_ = 0;
00180   this->aeul_ = 0;
00181 
00182   // start custom queue for imu
00183   this->callback_queue_thread_ =
00184     boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));
00185 
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::ConnectWorldUpdateBegin(
00191       boost::bind(&GazeboRosIMU::UpdateChild, this));
00192 }
00193 
00195 // returns true always, imu is always calibrated in sim
00196 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
00197                                         std_srvs::Empty::Response &res)
00198 {
00199   return true;
00200 }
00201 
00203 // Update the controller
00204 void GazeboRosIMU::UpdateChild()
00205 {
00206   common::Time cur_time = this->world_->GetSimTime();
00207   
00208   // rate control
00209   if (this->update_rate_ > 0 &&
00210       (cur_time - this->last_time_).Double() < (1.0 / this->update_rate_))
00211     return;
00212     
00213   if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
00214   {
00215     math::Pose pose;
00216     math::Quaternion rot;
00217     math::Vector3 pos;
00218 
00219     // Get Pose/Orientation ///@todo: verify correctness
00220     pose = this->link->GetWorldPose();
00221     // apply xyz offsets and get position and rotation components
00222     pos = pose.pos + this->offset_.pos;
00223     rot = pose.rot;
00224 
00225     // apply rpy offsets
00226     rot = this->offset_.rot*rot;
00227     rot.Normalize();
00228 
00229     // get Rates
00230     math::Vector3 vpos = this->link->GetWorldLinearVel();
00231     math::Vector3 veul = this->link->GetWorldAngularVel();
00232 
00233     // differentiate to get accelerations
00234     double tmp_dt = this->last_time_.Double() - cur_time.Double();
00235     if (tmp_dt != 0)
00236     {
00237       this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
00238       this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
00239       this->last_vpos_ = vpos;
00240       this->last_veul_ = veul;
00241     }
00242 
00243     // copy data into pose message
00244     this->imu_msg_.header.frame_id = this->frame_name_;
00245     this->imu_msg_.header.stamp.sec = cur_time.sec;
00246     this->imu_msg_.header.stamp.nsec = cur_time.nsec;
00247 
00248     // orientation quaternion
00249 
00250     // uncomment this if we are reporting orientation in the local frame
00251     // not the case for our imu definition
00252     // // apply fixed orientation offsets of initial pose
00253     // rot = this->initial_pose_.rot*rot;
00254     // rot.Normalize();
00255 
00256     this->imu_msg_.orientation.x = rot.x;
00257     this->imu_msg_.orientation.y = rot.y;
00258     this->imu_msg_.orientation.z = rot.z;
00259     this->imu_msg_.orientation.w = rot.w;
00260 
00261     // pass euler angular rates
00262     math::Vector3 linear_velocity(
00263       veul.x + this->GaussianKernel(0, this->gaussian_noise_),
00264       veul.y + this->GaussianKernel(0, this->gaussian_noise_),
00265       veul.z + this->GaussianKernel(0, this->gaussian_noise_));
00266     // rotate into local frame
00267     // @todo: deal with offsets!
00268     linear_velocity = rot.RotateVector(linear_velocity);
00269     this->imu_msg_.angular_velocity.x    = linear_velocity.x;
00270     this->imu_msg_.angular_velocity.y    = linear_velocity.y;
00271     this->imu_msg_.angular_velocity.z    = linear_velocity.z;
00272 
00273     // pass accelerations
00274     math::Vector3 linear_acceleration(
00275       apos_.x + this->GaussianKernel(0, this->gaussian_noise_),
00276       apos_.y + this->GaussianKernel(0, this->gaussian_noise_),
00277       apos_.z + this->GaussianKernel(0, this->gaussian_noise_));
00278     // rotate into local frame
00279     // @todo: deal with offsets!
00280     linear_acceleration = rot.RotateVector(linear_acceleration);
00281     this->imu_msg_.linear_acceleration.x    = linear_acceleration.x;
00282     this->imu_msg_.linear_acceleration.y    = linear_acceleration.y;
00283     this->imu_msg_.linear_acceleration.z    = linear_acceleration.z;
00284 
00285     // fill in covariance matrix
00288     double gn2 = this->gaussian_noise_*this->gaussian_noise_;
00289     this->imu_msg_.orientation_covariance[0] = gn2;
00290     this->imu_msg_.orientation_covariance[4] = gn2;
00291     this->imu_msg_.orientation_covariance[8] = gn2;
00292     this->imu_msg_.angular_velocity_covariance[0] = gn2;
00293     this->imu_msg_.angular_velocity_covariance[4] = gn2;
00294     this->imu_msg_.angular_velocity_covariance[8] = gn2;
00295     this->imu_msg_.linear_acceleration_covariance[0] = gn2;
00296     this->imu_msg_.linear_acceleration_covariance[4] = gn2;
00297     this->imu_msg_.linear_acceleration_covariance[8] = gn2;
00298 
00299     {
00300       boost::mutex::scoped_lock lock(this->lock_);
00301       // publish to ros
00302       if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
00303           this->pub_Queue->push(this->imu_msg_, this->pub_);
00304     }
00305 
00306     // save last time stamp
00307     this->last_time_ = cur_time;
00308   }
00309 }
00310 
00311 
00313 // Utility for adding noise
00314 double GazeboRosIMU::GaussianKernel(double mu, double sigma)
00315 {
00316   // using Box-Muller transform to generate two independent standard
00317   // normally disbributed normal variables see wikipedia
00318 
00319   // normalized uniform random variable
00320   double U = static_cast<double>(rand_r(&this->seed)) /
00321              static_cast<double>(RAND_MAX);
00322 
00323   // normalized uniform random variable
00324   double V = static_cast<double>(rand_r(&this->seed)) /
00325              static_cast<double>(RAND_MAX);
00326 
00327   double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
00328   // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
00329 
00330   // there are 2 indep. vars, we'll just use X
00331   // scale to our mu and sigma
00332   X = sigma * X + mu;
00333   return X;
00334 }
00335 
00337 // Put laser data to the interface
00338 void GazeboRosIMU::IMUQueueThread()
00339 {
00340   static const double timeout = 0.01;
00341 
00342   while (this->rosnode_->ok())
00343   {
00344     this->imu_queue_.callAvailable(ros::WallDuration(timeout));
00345   }
00346 }
00347 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22