gazebo_ros_ft_sensor.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2012-2014 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: Force Torque Sensor Plugin
00019  * Author: Francisco Suarez-Ruiz
00020  * Date: 5 June 2014
00021  */
00022 
00023 #include <gazebo_plugins/gazebo_ros_ft_sensor.h>
00024 #include <tf/tf.h>
00025 
00026 namespace gazebo
00027 {
00028 GZ_REGISTER_MODEL_PLUGIN(GazeboRosFT);
00029 
00031 // Constructor
00032 GazeboRosFT::GazeboRosFT()
00033 {
00034   this->ft_connect_count_ = 0;
00035   this->seed = 0;
00036 }
00037 
00039 // Destructor
00040 GazeboRosFT::~GazeboRosFT()
00041 {
00042   event::Events::DisconnectWorldUpdateBegin(this->update_connection_);
00043   // Custom Callback Queue
00044   this->queue_.clear();
00045   this->queue_.disable();
00046   this->rosnode_->shutdown();
00047   this->callback_queue_thread_.join();
00048   delete this->rosnode_;
00049 }
00050 
00052 // Load the controller
00053 void GazeboRosFT::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf )
00054 {
00055   // Save pointers
00056   this->model_ = _model;
00057   this->world_ = this->model_->GetWorld();
00058   
00059   // load parameters
00060   this->robot_namespace_ = "";
00061   if (_sdf->HasElement("robotNamespace"))
00062     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00063 
00064   if (!_sdf->HasElement("jointName"))
00065   {
00066     ROS_FATAL("ft_sensor plugin missing <jointName>, cannot proceed");
00067     return;
00068   }
00069   else
00070     this->joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
00071 
00072   this->joint_ = this->model_->GetJoint(this->joint_name_);
00073   if (!this->joint_)
00074   {
00075     ROS_FATAL("gazebo_ros_ft_sensor plugin error: jointName: %s does not exist\n",this->joint_name_.c_str());
00076     return;
00077   }
00078   
00079   this->parent_link_ = this->joint_->GetParent();
00080   this->child_link_ = this->joint_->GetChild();
00081   this->frame_name_ = this->child_link_->GetName();
00082   
00083   ROS_INFO("ft_sensor plugin reporting wrench values to the frame [%s]", this->frame_name_.c_str());
00084 
00085   if (!_sdf->HasElement("topicName"))
00086   {
00087     ROS_FATAL("ft_sensor plugin missing <topicName>, cannot proceed");
00088     return;
00089   }
00090   else
00091     this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
00092   
00093   if (!_sdf->HasElement("gaussianNoise"))
00094   {
00095     ROS_INFO("imu plugin missing <gaussianNoise>, defaults to 0.0");
00096     this->gaussian_noise_ = 0.0;
00097   }
00098   else
00099     this->gaussian_noise_ = _sdf->Get<double>("gaussianNoise");
00100   
00101   if (!_sdf->HasElement("updateRate"))
00102   {
00103     ROS_DEBUG("ft_sensor plugin missing <updateRate>, defaults to 0.0"
00104              " (as fast as possible)");
00105     this->update_rate_ = 0;
00106   }
00107   else
00108     this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
00109 
00110   // Make sure the ROS node for Gazebo has already been initialized
00111   if (!ros::isInitialized())
00112   {
00113     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00114       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00115     return;
00116   }
00117 
00118   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00119   
00120   // resolve tf prefix
00121   std::string prefix;
00122   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00123   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00124 
00125   // Custom Callback Queue
00126   ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
00127     this->topic_name_,1,
00128     boost::bind( &GazeboRosFT::FTConnect,this),
00129     boost::bind( &GazeboRosFT::FTDisconnect,this), ros::VoidPtr(), &this->queue_);
00130   this->pub_ = this->rosnode_->advertise(ao);
00131   
00132   // Custom Callback Queue
00133   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosFT::QueueThread,this ) );
00134   
00135   // New Mechanism for Updating every World Cycle
00136   // Listen to the update event. This event is broadcast every
00137   // simulation iteration.
00138   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00139       boost::bind(&GazeboRosFT::UpdateChild, this));
00140 }
00141 
00143 // Someone subscribes to me
00144 void GazeboRosFT::FTConnect()
00145 {
00146   this->ft_connect_count_++;
00147 }
00148 
00150 // Someone subscribes to me
00151 void GazeboRosFT::FTDisconnect()
00152 {
00153   this->ft_connect_count_--;
00154 }
00155 
00157 // Update the controller
00158 void GazeboRosFT::UpdateChild()
00159 {
00160   common::Time cur_time = this->world_->GetSimTime();
00161 
00162   // rate control
00163   if (this->update_rate_ > 0 &&
00164       (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
00165     return;
00166     
00167   if (this->ft_connect_count_ == 0)
00168     return;
00169 
00170   physics::JointWrench wrench;
00171   math::Vector3 torque;
00172   math::Vector3 force;
00173 
00174   // FIXME: Should include options for diferent frames and measure directions
00175   // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh
00176   // Get force torque at the joint
00177   // The wrench is reported in the CHILD <frame>
00178   // The <measure_direction> is child_to_parent
00179   wrench = this->joint_->GetForceTorque(0);
00180   force = wrench.body2Force;
00181   torque = wrench.body2Torque;
00182 
00183 
00184   this->lock_.lock();
00185   // copy data into wrench message
00186   this->wrench_msg_.header.frame_id = this->frame_name_;
00187   this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec;
00188   this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec;
00189 
00190   this->wrench_msg_.wrench.force.x = force.x + this->GaussianKernel(0, this->gaussian_noise_);
00191   this->wrench_msg_.wrench.force.y = force.y + this->GaussianKernel(0, this->gaussian_noise_);
00192   this->wrench_msg_.wrench.force.z = force.z + this->GaussianKernel(0, this->gaussian_noise_);
00193   this->wrench_msg_.wrench.torque.x = torque.x + this->GaussianKernel(0, this->gaussian_noise_);
00194   this->wrench_msg_.wrench.torque.y = torque.y + this->GaussianKernel(0, this->gaussian_noise_);
00195   this->wrench_msg_.wrench.torque.z = torque.z + this->GaussianKernel(0, this->gaussian_noise_);
00196 
00197   this->pub_.publish(this->wrench_msg_);
00198   this->lock_.unlock();
00199   
00200   // save last time stamp
00201   this->last_time_ = cur_time;
00202 }
00203 
00205 // Utility for adding noise
00206 double GazeboRosFT::GaussianKernel(double mu, double sigma)
00207 {
00208   // using Box-Muller transform to generate two independent standard
00209   // normally disbributed normal variables see wikipedia
00210 
00211   // normalized uniform random variable
00212   double U = static_cast<double>(rand_r(&this->seed)) /
00213              static_cast<double>(RAND_MAX);
00214 
00215   // normalized uniform random variable
00216   double V = static_cast<double>(rand_r(&this->seed)) /
00217              static_cast<double>(RAND_MAX);
00218 
00219   double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
00220   // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
00221 
00222   // there are 2 indep. vars, we'll just use X
00223   // scale to our mu and sigma
00224   X = sigma * X + mu;
00225   return X;
00226 }
00227 
00228 // Custom Callback Queue
00230 // custom callback queue thread
00231 void GazeboRosFT::QueueThread()
00232 {
00233   static const double timeout = 0.01;
00234 
00235   while (this->rosnode_->ok())
00236   {
00237     this->queue_.callAvailable(ros::WallDuration(timeout));
00238   }
00239 }
00240 
00241 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25