GetPoseHandler.cpp
Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <rigid_body_handler/GetPoseHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 #include <geometry_msgs/PoseStamped.h>
00009 #include <Eigen/Geometry>
00010 
00011 #include <vrep_ros_plugin/ConsoleHandler.h>
00012 
00013 
00014 GetPoseHandler::GetPoseHandler() : GenericObjectHandler(),
00015     _ObjPoseFrequency(-1),
00016     _lastPublishedObjPoseTime(0.0){
00017 }
00018 
00019 GetPoseHandler::~GetPoseHandler(){
00020 }
00021 
00022 unsigned int GetPoseHandler::getObjectType() const {
00023     return CustomDataHeaders::OBJ_POSE_DATA_MAIN;
00024 }
00025 
00026 void GetPoseHandler::synchronize(){
00027     // We update GetPoseHandler's data from its associated scene object custom data:
00028     // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00029     int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00030     char* datBuff=new char[buffSize];
00031     simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00032     std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00033     delete[] datBuff;
00034     // 2. From that retrieved data, try to extract sub-data with the OBJ_POSE_DATA_MAIN tag:
00035     std::vector<unsigned char> tempMainData;
00036 
00037 
00038     if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::OBJ_POSE_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00039 
00040         // Remove # chars for compatibility with ROS
00041                 _associatedObjectName = simGetObjectName(_associatedObjectID);
00042                 std::string objectName(_associatedObjectName);
00043                 std::replace( objectName.begin(), objectName.end(), '#', '_');
00044                 _pub = _nh.advertise<geometry_msgs::PoseStamped>(objectName+"/pose", 1);
00045                 std::stringstream streamtemp;
00046 
00047 
00048         int temp_freq = CAccess::pop_int(tempMainData);
00049 
00050         if (temp_freq > 0){
00051                 _ObjPoseFrequency = temp_freq;
00052                 //std::cout << "Frequency Pose Target = " << _ObjPoseFrequency << std::endl;
00053                 streamtemp << "- [Pose Target] in '" << _associatedObjectName << "'. Frequency publisher: " << _ObjPoseFrequency << "." << 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 << "- [Pose Target] in '" << _associatedObjectName << "'. Frequency publisher: Simulation frequency. " << std::endl;
00059             ConsoleHandler::printInConsole(streamtemp);
00060         }
00061 
00062     }
00063 
00064 
00065 
00066 }
00067 
00068 void GetPoseHandler::handleSimulation(){
00069     // called when the main script calls: simHandleModule
00070     if(!_initialized){
00071         _initialize();
00072     }
00073 
00074     ros::Time now = ros::Time::now();
00075 
00076     const simFloat currentSimulationTime = simGetSimulationTime();
00077 
00078     if ((currentSimulationTime-_lastPublishedObjPoseTime) >= 1.0/_ObjPoseFrequency){
00079 
00080         Eigen::Matrix<simFloat, 3, 1> position;
00081         Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
00082 
00083         // Get Position and Orientation of the object
00084         if(simGetObjectPosition(_associatedObjectID, -1, position.data())!=-1 && simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
00085 
00086 //              // use only positive w (not necessary)
00087                 if (orientation.w()<0){
00088                      orientation.coeffs() *=-1;
00089                   }
00090 
00091                 // Fill the status msg
00092                         geometry_msgs::PoseStamped msg;
00093 
00094                         msg.pose.position.x = position[0];
00095                         msg.pose.position.y = position[1];
00096                         msg.pose.position.z = position[2];
00097                         msg.pose.orientation.w = orientation.w();
00098                         msg.pose.orientation.x = orientation.x();
00099                         msg.pose.orientation.y = orientation.y();
00100                         msg.pose.orientation.z = orientation.z();
00101 
00102                         msg.header.stamp = now;
00103                         _pub.publish(msg);
00104                         _lastPublishedObjPoseTime = currentSimulationTime;
00105 
00106         }
00107 
00108     }
00109 
00110 }
00111 
00112 
00113 void GetPoseHandler::_initialize(){
00114     if (_initialized)
00115         return;
00116 
00117 
00118     std::vector<int> toExplore;
00119     toExplore.push_back(_associatedObjectID); // We start exploration with the base of the quadrotor-tree
00120     while (toExplore.size()!=0)
00121     {
00122         int objHandle=toExplore[toExplore.size()-1];
00123         toExplore.pop_back();
00124         // 1. Add this object's children to the list to explore:
00125         int index=0;
00126         int childHandle=simGetObjectChild(objHandle,index++);
00127         while (childHandle!=-1) {
00128             toExplore.push_back(childHandle);
00129 //            std::cout << "Adding " << simGetObjectName(childHandle) << " to exploration list." << std::endl;
00130             childHandle=simGetObjectChild(objHandle,index++);
00131         }
00132         // 2. Now check if this object has one of the tags we are looking for:
00133         // a. Get all the developer data attached to this scene object (this is custom data added by the developer):
00134         int buffSize=simGetObjectCustomDataLength(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER);
00135         if (buffSize!=0) { // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
00136             char* datBuff=new char[buffSize];
00137             simGetObjectCustomData(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00138             std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00139             delete[] datBuff;
00140             // b. From that retrieved data, try to extract sub-data with the searched tags:
00141             std::vector<unsigned char> quadrotorTagData;
00142 
00143 
00144         }
00145     }
00146    _lastPublishedObjPoseTime = -1e5;
00147     _initialized=true;
00148 }
00149 
00150 PLUGINLIB_EXPORT_CLASS(GetPoseHandler, GenericObjectHandler)


rigid_body_handler
Author(s): Riccardo Spica , Giovanni Claudio
autogenerated on Sat Jun 8 2019 20:22:55