gazebo_ros_bumper.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: Bumper controller
00020  * Author: Nate Koenig
00021  * Date: 09 Sept. 2008
00022  */
00023 
00024 #include <map>
00025 #include <string>
00026 
00027 #include <gazebo/physics/World.hh>
00028 #include <gazebo/physics/HingeJoint.hh>
00029 #include <gazebo/physics/Contact.hh>
00030 #include <gazebo/sensors/Sensor.hh>
00031 #include <sdf/sdf.hh>
00032 #include <sdf/Param.hh>
00033 #include <gazebo/common/Exception.hh>
00034 #include <gazebo/sensors/SensorTypes.hh>
00035 #include <gazebo/math/Pose.hh>
00036 #include <gazebo/math/Quaternion.hh>
00037 #include <gazebo/math/Vector3.hh>
00038 
00039 #include <tf/tf.h>
00040 
00041 #include <gazebo_plugins/gazebo_ros_bumper.h>
00042 
00043 namespace gazebo
00044 {
00045 // Register this plugin with the simulator
00046 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper)
00047 
00048 
00049 // Constructor
00050 GazeboRosBumper::GazeboRosBumper() : SensorPlugin()
00051 {
00052 }
00053 
00055 // Destructor
00056 GazeboRosBumper::~GazeboRosBumper()
00057 {
00058   this->rosnode_->shutdown();
00059   this->callback_queue_thread_.join();
00060 
00061   delete this->rosnode_;
00062 }
00063 
00065 // Load the controller
00066 void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00067 {
00068   this->parentSensor = boost::dynamic_pointer_cast<sensors::ContactSensor>(_parent);
00069   if (!this->parentSensor)
00070   {
00071     ROS_ERROR("Contact sensor parent is not of type ContactSensor");
00072     return;
00073   }
00074 
00075   this->robot_namespace_ = "";
00076   if (_sdf->HasElement("robotNamespace"))
00077     this->robot_namespace_ =
00078       _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00079 
00080   // "publishing contact/collisions to this topic name: "
00081   //   << this->bumper_topic_name_ << std::endl;
00082   this->bumper_topic_name_ = "bumper_states";
00083   if (_sdf->GetElement("bumperTopicName"))
00084     this->bumper_topic_name_ =
00085       _sdf->GetElement("bumperTopicName")->Get<std::string>();
00086 
00087   // "transform contact/collisions pose, forces to this body (link) name: "
00088   //   << this->frame_name_ << std::endl;
00089   if (!_sdf->HasElement("frameName"))
00090   {
00091     ROS_INFO("bumper plugin missing <frameName>, defaults to world");
00092     this->frame_name_ = "world";
00093   }
00094   else
00095     this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
00096 
00097   // Make sure the ROS node for Gazebo has already been initialized
00098   if (!ros::isInitialized())
00099   {
00100     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00101       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00102     return;
00103   }
00104 
00105   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00106 
00107   // resolve tf prefix
00108   std::string prefix;
00109   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
00110   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
00111 
00112   this->contact_pub_ = this->rosnode_->advertise<gazebo_msgs::ContactsState>(
00113     std::string(this->bumper_topic_name_), 1);
00114 
00115   // Initialize
00116   // start custom queue for contact bumper
00117   this->callback_queue_thread_ = boost::thread(
00118       boost::bind(&GazeboRosBumper::ContactQueueThread, this));
00119 
00120   // Listen to the update event. This event is broadcast every
00121   // simulation iteration.
00122   this->update_connection_ = this->parentSensor->ConnectUpdated(
00123      boost::bind(&GazeboRosBumper::OnContact, this));
00124 
00125   // Make sure the parent sensor is active.
00126   this->parentSensor->SetActive(true);
00127 }
00128 
00130 // Update the controller
00131 void GazeboRosBumper::OnContact()
00132 {
00133   if (this->contact_pub_.getNumSubscribers() <= 0)
00134     return;
00135 
00136   msgs::Contacts contacts;
00137   contacts = this->parentSensor->GetContacts();
00139   this->contact_state_msg_.header.frame_id = this->frame_name_;
00140   this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(),
00141                                contacts.time().nsec());
00142 
00143 /*
00146 
00147   // if frameName specified is "world", "/map" or "map" report back
00148   // inertial values in the gazebo world
00149   physics::LinkPtr myFrame;
00150   if (myFrame == NULL && this->frame_name_ != "world" &&
00151     this->frame_name_ != "/map" && this->frame_name_ != "map")
00152   {
00153     // lock in case a model is being spawned
00154     boost::recursive_mutex::scoped_lock lock(
00155       *Simulator::Instance()->GetMRMutex());
00156     // look through all models in the world, search for body
00157     // name that matches frameName
00158     phyaics::Model_V all_models = World::Instance()->GetModels();
00159     for (physics::Model_V::iterator iter = all_models.begin();
00160       iter != all_models.end(); iter++)
00161     {
00162       if (*iter) myFrame =
00163         boost::dynamic_pointer_cast<physics::Link>((*iter)->GetLink(this->frame_name_));
00164       if (myFrame) break;
00165     }
00166 
00167     // not found
00168     if (!myFrame)
00169     {
00170       ROS_INFO("gazebo_ros_bumper plugin: frameName: %s does not exist"
00171                 " yet, will not publish\n",this->frame_name_.c_str());
00172       return;
00173     }
00174   }
00175 */
00176   // get reference frame (body(link)) pose and subtract from it to get
00177   // relative force, torque, position and normal vectors
00178   math::Pose pose, frame_pose;
00179   math::Quaternion rot, frame_rot;
00180   math::Vector3 pos, frame_pos;
00181   /*
00182   if (myFrame)
00183   {
00184     frame_pose = myFrame->GetWorldPose();  //-this->myBody->GetCoMPose();
00185     frame_pos = frame_pose.pos;
00186     frame_rot = frame_pose.rot;
00187   }
00188   else
00189   */
00190   {
00191     // no specific frames specified, use identity pose, keeping
00192     // relative frame at inertial origin
00193     frame_pos = math::Vector3(0, 0, 0);
00194     frame_rot = math::Quaternion(1, 0, 0, 0);  // gazebo u,x,y,z == identity
00195     frame_pose = math::Pose(frame_pos, frame_rot);
00196   }
00197 
00198 
00199 
00200   // set contact states size
00201   this->contact_state_msg_.states.clear();
00202 
00203   // GetContacts returns all contacts on the collision body
00204   unsigned int contactsPacketSize = contacts.contact_size();
00205   for (unsigned int i = 0; i < contactsPacketSize; ++i)
00206   {
00207 
00208     // For each collision contact
00209     // Create a ContactState
00210     gazebo_msgs::ContactState state;
00212     gazebo::msgs::Contact contact = contacts.contact(i);
00213 
00214     state.collision1_name = contact.collision1();
00215     state.collision2_name = contact.collision2();
00216     std::ostringstream stream;
00217     stream << "Debug:  i:(" << i << "/" << contactsPacketSize
00218       << ")     my geom:" << state.collision1_name
00219       << "   other geom:" << state.collision2_name
00220       << "         time:" << ros::Time(contact.time().sec(), contact.time().nsec())
00221       << std::endl;
00222     state.info = stream.str();
00223 
00224     state.wrenches.clear();
00225     state.contact_positions.clear();
00226     state.contact_normals.clear();
00227     state.depths.clear();
00228 
00229     // sum up all wrenches for each DOF
00230     geometry_msgs::Wrench total_wrench;
00231     total_wrench.force.x = 0;
00232     total_wrench.force.y = 0;
00233     total_wrench.force.z = 0;
00234     total_wrench.torque.x = 0;
00235     total_wrench.torque.y = 0;
00236     total_wrench.torque.z = 0;
00237 
00238     unsigned int contactGroupSize = contact.position_size();
00239     for (unsigned int j = 0; j < contactGroupSize; ++j)
00240     {
00241       // loop through individual contacts between collision1 and collision2
00242       // gzerr << j << "  Position:"
00243       //       << contact.position(j).x() << " "
00244       //       << contact.position(j).y() << " "
00245       //       << contact.position(j).z() << "\n";
00246       // gzerr << "   Normal:"
00247       //       << contact.normal(j).x() << " "
00248       //       << contact.normal(j).y() << " "
00249       //       << contact.normal(j).z() << "\n";
00250       // gzerr << "   Depth:" << contact.depth(j) << "\n";
00251 
00252       // Get force, torque and rotate into user specified frame.
00253       // frame_rot is identity if world is used (default for now)
00254       math::Vector3 force = frame_rot.RotateVectorReverse(math::Vector3(
00255                               contact.wrench(j).body_1_wrench().force().x(),
00256                             contact.wrench(j).body_1_wrench().force().y(),
00257                             contact.wrench(j).body_1_wrench().force().z()));
00258       math::Vector3 torque = frame_rot.RotateVectorReverse(math::Vector3(
00259                             contact.wrench(j).body_1_wrench().torque().x(),
00260                             contact.wrench(j).body_1_wrench().torque().y(),
00261                             contact.wrench(j).body_1_wrench().torque().z()));
00262 
00263       // set wrenches
00264       geometry_msgs::Wrench wrench;
00265       wrench.force.x  = force.x;
00266       wrench.force.y  = force.y;
00267       wrench.force.z  = force.z;
00268       wrench.torque.x = torque.x;
00269       wrench.torque.y = torque.y;
00270       wrench.torque.z = torque.z;
00271       state.wrenches.push_back(wrench);
00272 
00273       total_wrench.force.x  += wrench.force.x;
00274       total_wrench.force.y  += wrench.force.y;
00275       total_wrench.force.z  += wrench.force.z;
00276       total_wrench.torque.x += wrench.torque.x;
00277       total_wrench.torque.y += wrench.torque.y;
00278       total_wrench.torque.z += wrench.torque.z;
00279 
00280       // transform contact positions into relative frame
00281       // set contact positions
00282       gazebo::math::Vector3 position = frame_rot.RotateVectorReverse(
00283           math::Vector3(contact.position(j).x(),
00284                         contact.position(j).y(),
00285                         contact.position(j).z()) - frame_pos);
00286       geometry_msgs::Vector3 contact_position;
00287       contact_position.x = position.x;
00288       contact_position.y = position.y;
00289       contact_position.z = position.z;
00290       state.contact_positions.push_back(contact_position);
00291 
00292       // rotate normal into user specified frame.
00293       // frame_rot is identity if world is used.
00294       math::Vector3 normal = frame_rot.RotateVectorReverse(
00295           math::Vector3(contact.normal(j).x(),
00296                         contact.normal(j).y(),
00297                         contact.normal(j).z()));
00298       // set contact normals
00299       geometry_msgs::Vector3 contact_normal;
00300       contact_normal.x = normal.x;
00301       contact_normal.y = normal.y;
00302       contact_normal.z = normal.z;
00303       state.contact_normals.push_back(contact_normal);
00304 
00305       // set contact depth, interpenetration
00306       state.depths.push_back(contact.depth(j));
00307     }
00308 
00309     state.total_wrench = total_wrench;
00310     this->contact_state_msg_.states.push_back(state);
00311   }
00312 
00313   this->contact_pub_.publish(this->contact_state_msg_);
00314 }
00315 
00316 
00318 // Put laser data to the interface
00319 void GazeboRosBumper::ContactQueueThread()
00320 {
00321   static const double timeout = 0.01;
00322 
00323   while (this->rosnode_->ok())
00324   {
00325     this->contact_queue_.callAvailable(ros::WallDuration(timeout));
00326   }
00327 }
00328 }


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