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


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