GetTwistHandler.cpp
Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <rigid_body_handler/GetTwistHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 
00009 #include <geometry_msgs/TwistStamped.h>
00010 #include <Eigen/Geometry>
00011 
00012 #include <vrep_ros_plugin/ConsoleHandler.h>
00013 
00014 
00015 GetTwistHandler::GetTwistHandler() : GenericObjectHandler(),
00016     _ObjTwistFrequency(-1),
00017     _lastPublishedObjTwistTime(0.0),
00018     _isStatic(true){
00019 }
00020 
00021 GetTwistHandler::~GetTwistHandler(){
00022 }
00023 
00024 unsigned int GetTwistHandler::getObjectType() const {
00025     return CustomDataHeaders::OBJ_TWIST_DATA_MAIN;
00026 }
00027 
00028 void GetTwistHandler::synchronize(){
00029     // We update GetTwistHandler's data from its associated scene object custom data:
00030     // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00031     int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00032     char* datBuff=new char[buffSize];
00033     simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00034     std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00035     delete[] datBuff;
00036     // 2. From that retrieved data, try to extract sub-data with the OBJ_TWIST_DATA_MAIN tag:
00037     std::vector<unsigned char> tempMainData;
00038 
00039     if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00040 
00041         // Remove # chars for compatibility with ROS
00042                 _associatedObjectName = simGetObjectName(_associatedObjectID);
00043                 std::string objectName(_associatedObjectName);
00044                 std::replace( objectName.begin(), objectName.end(), '#', '_');
00045                 _pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
00046                 std::stringstream streamtemp;
00047 
00048         int temp_freq = CAccess::pop_int(tempMainData);
00049         if (temp_freq > 0){
00050                 _ObjTwistFrequency = temp_freq;
00051                 //std::cout << "Found Twist target in " << _associatedObjectName << ". Frequency publisher: " << _ObjTwistFrequency << std::endl;
00052 
00053                 streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: " << _ObjTwistFrequency << "." << std::endl;
00054                 ConsoleHandler::printInConsole(streamtemp);
00055         } else {
00056                 //std::cout << "Found Twist target in " << _associatedObjectName << " at the simulation frequency. " << std::endl;
00057 
00058                 streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: Simulation frequency. " << std::endl;
00059             ConsoleHandler::printInConsole(streamtemp);
00060         }
00061     }
00062 
00063 
00064 }
00065 
00066 void GetTwistHandler::handleSimulation(){
00067     // called when the main script calls: simHandleModule
00068     if(!_initialized){
00069         _initialize();
00070     }
00071 
00072     ros::Time now = ros::Time::now();
00073 
00074 
00075     const simFloat currentSimulationTime = simGetSimulationTime();
00076 
00077     if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){
00078 
00079         Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
00080         Eigen::Matrix<simFloat, 3, 1> linVelocity;
00081         Eigen::Matrix<simFloat, 3, 1> angVelocity;
00082         bool error = false;
00083 
00084         // Get object velocity. If the object is not static simGetVelocity is more accurate.
00085         if (_isStatic){
00086                 error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
00087         } else {
00088                 error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
00089         }
00090 
00091         // Get object orientation
00092         error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;
00093 
00094         if(!error){
00095 
00096                 linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
00097                         angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame
00098 
00099                 // Fill the status msg
00100                         geometry_msgs::TwistStamped msg;
00101 
00102                         msg.twist.linear.x = linVelocity[0];
00103                         msg.twist.linear.y = linVelocity[1];
00104                         msg.twist.linear.z = linVelocity[2];
00105                         msg.twist.angular.x = angVelocity[0];
00106                         msg.twist.angular.y = angVelocity[1];
00107                         msg.twist.angular.z = angVelocity[2];
00108 
00109                         msg.header.stamp = now;
00110                         _pub.publish(msg);
00111                         _lastPublishedObjTwistTime = currentSimulationTime;
00112 
00113         } else {
00114             std::stringstream ss;
00115             ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
00116             ConsoleHandler::printInConsole(ss);
00117         }
00118 
00119     }
00120 
00121 }
00122 
00123 
00124 void GetTwistHandler::_initialize(){
00125     if (_initialized)
00126         return;
00127 
00128 
00129     std::vector<int> toExplore;
00130     toExplore.push_back(_associatedObjectID); // We start exploration with the base of the quadrotor-tree
00131     while (toExplore.size()!=0)
00132     {
00133         int objHandle=toExplore[toExplore.size()-1];
00134         toExplore.pop_back();
00135         // 1. Add this object's children to the list to explore:
00136         int index=0;
00137         int childHandle=simGetObjectChild(objHandle,index++);
00138         while (childHandle!=-1) {
00139             toExplore.push_back(childHandle);
00140 //            std::cout << "Adding " << simGetObjectName(childHandle) << " to exploration list." << std::endl;
00141             childHandle=simGetObjectChild(objHandle,index++);
00142         }
00143         // 2. Now check if this object has one of the tags we are looking for:
00144         // a. Get all the developer data attached to this scene object (this is custom data added by the developer):
00145         int buffSize=simGetObjectCustomDataLength(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER);
00146         if (buffSize!=0) { // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
00147             char* datBuff=new char[buffSize];
00148             simGetObjectCustomData(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00149             std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00150             delete[] datBuff;
00151             // b. From that retrieved data, try to extract sub-data with the searched tags:
00152             std::vector<unsigned char> quadrotorTagData;
00153 
00154 
00155         }
00156     }
00157     _lastPublishedObjTwistTime = -1e5;
00158 
00159     //Check if the object is static
00160     simGetObjectIntParameter(_associatedObjectID, 3003, &_isStatic);
00161     if(_isStatic){
00162                 std::stringstream ss;
00163                 ss << "- [" << _associatedObjectName << "] WARNING: getting velocity of a static object might give inaccurate results." << std::endl;;
00164                 ConsoleHandler::printInConsole(ss);
00165     }
00166 
00167     _initialized=true;
00168 
00169 }
00170 
00171 
00172 PLUGINLIB_EXPORT_CLASS(GetTwistHandler, GenericObjectHandler)


rigid_body_handler
Author(s): Riccardo Spica , Giovanni Claudio
autogenerated on Mon Apr 3 2017 04:03:58