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


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