gazebo_ros_force.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: GazeboRosForce plugin for manipulating objects in Gazebo
00024    Author: John Hsu
00025    Date: 24 Sept 2008
00026    SVN info: $Id$
00027  @htmlinclude manifest.html
00028  @b GazeboRosForce plugin reads ROS geometry_msgs/Wrench messages
00029  */
00030 
00031 #include <algorithm>
00032 #include <assert.h>
00033 
00034 #include <gazebo_plugins/gazebo_ros_force.h>
00035 
00036 namespace gazebo
00037 {
00038 
00040 // Constructor
00041 GazeboRosForce::GazeboRosForce()
00042 {
00043   this->wrench_msg_.force.x = 0;
00044   this->wrench_msg_.force.y = 0;
00045   this->wrench_msg_.force.z = 0;
00046   this->wrench_msg_.torque.x = 0;
00047   this->wrench_msg_.torque.y = 0;
00048   this->wrench_msg_.torque.z = 0;
00049 }
00050 
00052 // Destructor
00053 GazeboRosForce::~GazeboRosForce()
00054 {
00055   event::Events::DisconnectWorldUpdateStart(this->update_connection_);
00056 
00057   // Custom Callback Queue
00058   this->queue_.clear();
00059   this->queue_.disable();
00060   this->rosnode_->shutdown();
00061   this->callback_queue_thread_.join();
00062 
00063   delete this->rosnode_;
00064 }
00065 
00067 // Load the controller
00068 void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00069 {
00070   // Get the world name.
00071   this->world_ = _model->GetWorld();
00072   
00073   // load parameters
00074   this->robot_namespace_ = "";
00075   if (_sdf->HasElement("robotNamespace"))
00076     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00077 
00078   if (!_sdf->HasElement("bodyName"))
00079   {
00080     ROS_FATAL("f3d plugin missing <bodyName>, cannot proceed");
00081     return;
00082   }
00083   else
00084     this->link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00085 
00086   this->link_ = _model->GetLink(this->link_name_);
00087   if (!this->link_)
00088   {
00089     ROS_FATAL("gazebo_ros_f3d plugin error: link named: %s does not exist\n",this->link_name_.c_str());
00090     return;
00091   }
00092 
00093   if (!_sdf->HasElement("topicName"))
00094   {
00095     ROS_FATAL("f3d plugin missing <topicName>, cannot proceed");
00096     return;
00097   }
00098   else
00099     this->topic_name_ = _sdf->GetElement("topicName")->GetValueString();
00100 
00101 
00102   // initialize ros if not done so already
00103   if (!ros::isInitialized())
00104   {
00105     ROS_FATAL("while loading gazebo_ros_force plugin, ros is not initialized, please load a gazebo system plugin that initializes ros (e.g. libgazebo_ros_api_plugin.so from gazebo ros package)\n");
00106     return;
00107   }
00108 
00109   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00110 
00111   // Custom Callback Queue
00112   ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
00113     this->topic_name_,1,
00114     boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1),
00115     ros::VoidPtr(), &this->queue_);
00116   this->sub_ = this->rosnode_->subscribe(so);
00117 
00118   // Custom Callback Queue
00119   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) );
00120 
00121   // New Mechanism for Updating every World Cycle
00122   // Listen to the update event. This event is broadcast every
00123   // simulation iteration.
00124   this->update_connection_ = event::Events::ConnectWorldUpdateStart(
00125       boost::bind(&GazeboRosForce::UpdateChild, this));
00126 }
00127 
00129 // Update the controller
00130 void GazeboRosForce::UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg)
00131 {
00132   this->wrench_msg_.force.x = _msg->force.x;
00133   this->wrench_msg_.force.y = _msg->force.y;
00134   this->wrench_msg_.force.z = _msg->force.z;
00135   this->wrench_msg_.torque.x = _msg->torque.x;
00136   this->wrench_msg_.torque.y = _msg->torque.y;
00137   this->wrench_msg_.torque.z = _msg->torque.z;
00138 }
00139 
00141 // Update the controller
00142 void GazeboRosForce::UpdateChild()
00143 {
00144   this->lock_.lock();
00145   math::Vector3 force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z);
00146   math::Vector3 torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z);
00147   this->link_->SetForce(force);
00148   this->link_->SetTorque(torque);
00149   this->lock_.unlock();
00150 }
00151 
00152 // Custom Callback Queue
00154 // custom callback queue thread
00155 void GazeboRosForce::QueueThread()
00156 {
00157   static const double timeout = 0.01;
00158 
00159   while (this->rosnode_->ok())
00160   {
00161     this->queue_.callAvailable(ros::WallDuration(timeout));
00162   }
00163 }
00164 
00165 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce);
00166 
00167 }


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