gazebo_ros_f3d.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: Force Feed Back Ground Truth
00020    Author: Sachin Chitta and John Hsu
00021    Date: 1 June 2008
00022  */
00023 
00024 #include <gazebo_plugins/gazebo_ros_f3d.h>
00025 #include <tf/tf.h>
00026 
00027 namespace gazebo
00028 {
00029 GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D);
00030 
00032 // Constructor
00033 GazeboRosF3D::GazeboRosF3D()
00034 {
00035   this->f3d_connect_count_ = 0;
00036 }
00037 
00039 // Destructor
00040 GazeboRosF3D::~GazeboRosF3D()
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 GazeboRosF3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00054 {
00055   // Get the world name.
00056   this->world_ = _parent->GetWorld();
00057   
00058   // load parameters
00059   this->robot_namespace_ = "";
00060   if (_sdf->HasElement("robotNamespace"))
00061     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00062 
00063   if (!_sdf->HasElement("bodyName"))
00064   {
00065     ROS_FATAL("f3d plugin missing <bodyName>, cannot proceed");
00066     return;
00067   }
00068   else
00069     this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00070 
00071   this->link_ = _parent->GetLink(this->link_name_);
00072   if (!this->link_)
00073   {
00074     ROS_FATAL("gazebo_ros_f3d plugin error: bodyName: %s does not exist\n",this->link_name_.c_str());
00075     return;
00076   }
00077 
00078   if (!_sdf->HasElement("topicName"))
00079   {
00080     ROS_FATAL("f3d 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_INFO("f3d plugin missing <frameName>, defaults to world");
00089     this->frame_name_ = "world";
00090   }
00091   else
00092   {
00093     this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
00094     // todo: frameName not used
00095     ROS_INFO("f3d plugin specifies <frameName> [%s], not used, default to world",this->frame_name_.c_str());
00096   }
00097   
00098 
00099   // Make sure the ROS node for Gazebo has already been initialized
00100   if (!ros::isInitialized())
00101   {
00102     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00103       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00104     return;
00105   }
00106 
00107   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00108   
00109   // resolve tf prefix
00110   std::string prefix;
00111   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00112   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00113 
00114   // Custom Callback Queue
00115   ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
00116     this->topic_name_,1,
00117     boost::bind( &GazeboRosF3D::F3DConnect,this),
00118     boost::bind( &GazeboRosF3D::F3DDisconnect,this), ros::VoidPtr(), &this->queue_);
00119   this->pub_ = this->rosnode_->advertise(ao);
00120   
00121   // Custom Callback Queue
00122   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosF3D::QueueThread,this ) );
00123   
00124   // New Mechanism for Updating every World Cycle
00125   // Listen to the update event. This event is broadcast every
00126   // simulation iteration.
00127   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00128       boost::bind(&GazeboRosF3D::UpdateChild, this));
00129 }
00130 
00132 // Someone subscribes to me
00133 void GazeboRosF3D::F3DConnect()
00134 {
00135   this->f3d_connect_count_++;
00136 }
00137 
00139 // Someone subscribes to me
00140 void GazeboRosF3D::F3DDisconnect()
00141 {
00142   this->f3d_connect_count_--;
00143 }
00144 
00146 // Update the controller
00147 void GazeboRosF3D::UpdateChild()
00148 {
00149   if (this->f3d_connect_count_ == 0)
00150     return;
00151 
00152   math::Vector3 torque;
00153   math::Vector3 force;
00154 
00155   // get force on body
00156   force = this->link_->GetWorldForce();
00157 
00158   // get torque on body
00159   torque = this->link_->GetWorldTorque();
00160 
00161   this->lock_.lock();
00162   // copy data into wrench message
00163   this->wrench_msg_.header.frame_id = this->frame_name_;
00164   this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec;
00165   this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec;
00166 
00167   this->wrench_msg_.wrench.force.x    = force.x;
00168   this->wrench_msg_.wrench.force.y    = force.y;
00169   this->wrench_msg_.wrench.force.z    = force.z;
00170   this->wrench_msg_.wrench.torque.x   = torque.x;
00171   this->wrench_msg_.wrench.torque.y   = torque.y;
00172   this->wrench_msg_.wrench.torque.z   = torque.z;
00173 
00174   this->pub_.publish(this->wrench_msg_);
00175   this->lock_.unlock();
00176 
00177 }
00178 
00179 // Custom Callback Queue
00181 // custom callback queue thread
00182 void GazeboRosF3D::QueueThread()
00183 {
00184   static const double timeout = 0.01;
00185 
00186   while (this->rosnode_->ok())
00187   {
00188     this->queue_.callAvailable(ros::WallDuration(timeout));
00189   }
00190 }
00191 
00192 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09