ContactHandler.cpp
Go to the documentation of this file.
00001 #include <pluginlib/class_list_macros.h>
00002 
00003 #include <contact_handler/ContactHandler.h>
00004 #include <v_repLib.h>
00005 #include <vrep_ros_plugin/access.h>
00006 
00007 #include <geometry_msgs/Wrench.h>
00008 #include <geometry_msgs/Vector3.h>
00009 #include <gazebo_msgs/ContactsState.h>
00010 
00011 #include <vrep_ros_plugin/ConsoleHandler.h>
00012 
00013 
00014 ContactHandler::ContactHandler() : GenericObjectHandler(),
00015     _lastPublishedContact(0.0),
00016     _acquisitionFrequency(-1.0)
00017   {
00018 }
00019 
00020 ContactHandler::~ContactHandler(){
00021 }
00022 
00023 unsigned int ContactHandler::getObjectType() const {
00024     return CustomDataHeaders::CONTACT_DATA_MAIN;
00025 }
00026 
00027 void ContactHandler::synchronize(){
00028     _associatedObjectName = simGetObjectName(_associatedObjectID);
00029 }
00030 
00031 void ContactHandler::handleSimulation(){
00032     // called when the main script calls: simHandleModule
00033     if(!_initialized){
00034         _initialize();
00035     }
00036 
00037     const simFloat currentSimulationTime = simGetSimulationTime();
00038 
00039     if ( ((currentSimulationTime - _lastPublishedContact) >= 1.0/_acquisitionFrequency) ){
00040 
00041         gazebo_msgs::ContactsState contactsMessage;
00042 
00043         simInt contactHandles[2];
00044         simFloat contactInfo[6];
00045 
00046         // simGetContactInfo is asked for contacts by their indices; if there is no contact with the given index,
00047         // the function returns 0 and we know that we have already iterated through all contacts
00048         int contact_index = 0;
00049         while (0 < simGetContactInfo(sim_handle_all, sim_handle_all, contact_index, contactHandles, contactInfo)) {
00050 
00051                         // Construct the contact msg
00052 
00053             gazebo_msgs::ContactState contact;
00054 
00055             contact.collision1_name = std::string(simGetObjectName(contactHandles[0]));
00056             contact.collision2_name = std::string(simGetObjectName(contactHandles[1]));
00057 
00058             geometry_msgs::Vector3 contactPosition;
00059             contactPosition.x = contactInfo[0]; contactPosition.y = contactInfo[1]; contactPosition.z = contactInfo[2];
00060             contact.contact_positions.push_back(contactPosition);
00061 
00062             geometry_msgs::Vector3 force;
00063             force.x = contactInfo[3];
00064             force.y = contactInfo[4];
00065             force.z = contactInfo[5];
00066             contact.contact_normals.push_back(force);
00067 
00068                         geometry_msgs::Wrench wrench;
00069                         wrench.force = force;
00070             // wrench.torque = ???; // there is no torque info AFAIK
00071                         contact.wrenches.push_back(wrench);
00072             contact.total_wrench = wrench;
00073 
00074             contactsMessage.states.push_back(contact);
00075 
00076             ++contact_index;
00077                 }
00078 
00079         // Publish the contact message in ROS
00080         _pub.publish(contactsMessage);
00081         _lastPublishedContact = currentSimulationTime;
00082     }
00083 }
00084 
00085 void ContactHandler::_initialize(){
00086     if (_initialized)
00087         return;
00088 
00089     // start the ROS publisher
00090     _pub = _nh.advertise<gazebo_msgs::ContactsState>("contacts", 1000);
00091 
00092     // get some data from the main object
00093     std::vector<unsigned char> developerCustomData;
00094     getDeveloperCustomData(developerCustomData);
00095 
00096     // 2. From that retrieved data, try to extract sub-data with the CONTACT_DATA_MAIN tag:
00097     std::vector<unsigned char> tempMainData;
00098     std::stringstream ss;
00099 
00100     if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::CONTACT_DATA_MAIN,tempMainData)){
00101         _acquisitionFrequency=CAccess::pop_float(tempMainData);
00102         if (_acquisitionFrequency > 0.0){
00103             ss << "- ContactHandler [" << _associatedObjectName << "] Frequency publisher: " << _acquisitionFrequency << "." << std::endl;
00104         } else {
00105             ss << "- ContactHandler [" << _associatedObjectName << "] Frequency publisher: 50 Hz."  << std::endl;
00106             _acquisitionFrequency = 50.0;
00107         }
00108     } else {
00109         ss << "- ContactHandler [" << _associatedObjectName << "] Contact frequency publisher not specified. Using 50 Hz as default."  << std::endl;
00110         _acquisitionFrequency = 50.0;
00111     }
00112 
00113     ConsoleHandler::printInConsole(ss);
00114 
00115     _lastPublishedContact = -1.0f;
00116     _initialized=true;
00117 }
00118 
00119 bool ContactHandler::endOfSimulation(){
00120 
00121     _initialized=false;
00122     _pub.shutdown();
00123 
00124     return false;
00125 }
00126 
00127 PLUGINLIB_EXPORT_CLASS(ContactHandler, GenericObjectHandler)


contact_handler
Author(s): Martin Pecka
autogenerated on Wed Sep 9 2015 18:54:58