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


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22