gazebo_ros_f3d.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  @mainpage
00023    Desc: Force Feed Back Ground Truth
00024    Author: Sachin Chitta and John Hsu
00025    Date: 1 June 2008
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosF3D plugin broadcasts forces acting on the body specified by name.
00029  */
00030 
00031 #include <gazebo_plugins/gazebo_ros_f3d.h>
00032 
00033 #include "tf/tf.h"
00034 
00035 namespace gazebo
00036 {
00037 
00039 // Constructor
00040 GazeboRosF3D::GazeboRosF3D()
00041 {
00042   this->f3d_connect_count_ = 0;
00043 }
00044 
00046 // Destructor
00047 GazeboRosF3D::~GazeboRosF3D()
00048 {
00049   event::Events::DisconnectWorldUpdateStart(this->update_connection_);
00050   // Custom Callback Queue
00051   this->queue_.clear();
00052   this->queue_.disable();
00053   this->rosnode_->shutdown();
00054   this->callback_queue_thread_.join();
00055   delete this->rosnode_;
00056 }
00057 
00059 // Load the controller
00060 void GazeboRosF3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00061 {
00062   // Get the world name.
00063   this->world_ = _parent->GetWorld();
00064   
00065   // load parameters
00066   this->robot_namespace_ = "";
00067   if (_sdf->HasElement("robotNamespace"))
00068     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00069 
00070   if (!_sdf->HasElement("bodyName"))
00071   {
00072     ROS_FATAL("f3d plugin missing <bodyName>, cannot proceed");
00073     return;
00074   }
00075   else
00076     this->link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00077 
00078   this->link_ = _parent->GetLink(this->link_name_);
00079   if (!this->link_)
00080   {
00081     ROS_FATAL("gazebo_ros_f3d plugin error: bodyName: %s does not exist\n",this->link_name_.c_str());
00082     return;
00083   }
00084 
00085   if (!_sdf->HasElement("topicName"))
00086   {
00087     ROS_FATAL("f3d plugin missing <topicName>, cannot proceed");
00088     return;
00089   }
00090   else
00091     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00092 
00093   if (!_sdf->HasElement("frameName"))
00094   {
00095     ROS_INFO("f3d plugin missing <frameName>, defaults to world");
00096     this->frame_name_ = "world";
00097   }
00098   else
00099   {
00100     this->frame_name_ = _sdf->GetElement("frameName")->GetValueString();
00101     // todo: frameName not used
00102     ROS_INFO("f3d plugin specifies <frameName> [%s], not used, default to world",this->frame_name_.c_str());
00103   }
00104   
00105   if (!ros::isInitialized())
00106   {
00107     int argc = 0;
00108     char** argv = NULL;
00109     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00110   }
00111 
00112   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00113   
00114   // resolve tf prefix
00115   std::string prefix;
00116   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00117   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00118 
00119   // Custom Callback Queue
00120   ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
00121     this->topic_name_,1,
00122     boost::bind( &GazeboRosF3D::F3DConnect,this),
00123     boost::bind( &GazeboRosF3D::F3DDisconnect,this), ros::VoidPtr(), &this->queue_);
00124   this->pub_ = this->rosnode_->advertise(ao);
00125   
00126   // Custom Callback Queue
00127   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosF3D::QueueThread,this ) );
00128   
00129   // New Mechanism for Updating every World Cycle
00130   // Listen to the update event. This event is broadcast every
00131   // simulation iteration.
00132   this->update_connection_ = event::Events::ConnectWorldUpdateStart(
00133       boost::bind(&GazeboRosF3D::UpdateChild, this));
00134 }
00135 
00137 // Someone subscribes to me
00138 void GazeboRosF3D::F3DConnect()
00139 {
00140   this->f3d_connect_count_++;
00141 }
00142 
00144 // Someone subscribes to me
00145 void GazeboRosF3D::F3DDisconnect()
00146 {
00147   this->f3d_connect_count_--;
00148 }
00149 
00151 // Update the controller
00152 void GazeboRosF3D::UpdateChild()
00153 {
00154   if (this->f3d_connect_count_ == 0)
00155     return;
00156 
00157   math::Vector3 torque;
00158   math::Vector3 force;
00159 
00160   // get force on body
00161   force = this->link_->GetWorldForce();
00162 
00163   // get torque on body
00164   torque = this->link_->GetWorldTorque();
00165 
00166   this->lock_.lock();
00167   // copy data into wrench message
00168   this->wrench_msg_.header.frame_id = this->frame_name_;
00169   this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec;
00170   this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec;
00171 
00172   this->wrench_msg_.wrench.force.x    = force.x;
00173   this->wrench_msg_.wrench.force.y    = force.y;
00174   this->wrench_msg_.wrench.force.z    = force.z;
00175   this->wrench_msg_.wrench.torque.x   = torque.x;
00176   this->wrench_msg_.wrench.torque.y   = torque.y;
00177   this->wrench_msg_.wrench.torque.z   = torque.z;
00178 
00179   this->pub_.publish(this->wrench_msg_);
00180   this->lock_.unlock();
00181 
00182 }
00183 
00184 // Custom Callback Queue
00186 // custom callback queue thread
00187 void GazeboRosF3D::QueueThread()
00188 {
00189   static const double timeout = 0.01;
00190 
00191   while (this->rosnode_->ok())
00192   {
00193     this->queue_.callAvailable(ros::WallDuration(timeout));
00194   }
00195 }
00196 
00197 GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D);
00198 
00199 }


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