ImuHandler.cpp
Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <imu_handler/ImuHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 #include <sensor_msgs/Imu.h>
00009 #include <Eigen/Geometry>
00010 
00011 #include <vrep_ros_plugin/ConsoleHandler.h>
00012 
00013 
00014 ImuHandler::ImuHandler() : GenericObjectHandler(),
00015     _handleOfMass(-1),
00016     _mass(1.0),
00017     _handleOfForceSensor(-1),
00018     _lastPublishedImu(0.0),
00019     _acquisitionFrequency(-1.0)
00020    // _forceFilterCutoff(-1.0)
00021 {
00022 }
00023 
00024 ImuHandler::~ImuHandler(){
00025 }
00026 
00027 unsigned int ImuHandler::getObjectType() const {
00028     return CustomDataHeaders::IMU_DATA_MAIN;
00029 }
00030 
00031 void ImuHandler::synchronize(){
00032 //    // We update ImuHandler's data from its associated scene object custom data:
00033 //    // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00034 //    int buffSize=simGetObjectCustomDataLength(_associatedObjectID,DEVELOPER_DATA_HEADER);
00035 //    char* datBuff=new char[buffSize];
00036 //    simGetObjectCustomData(_associatedObjectID,DEVELOPER_DATA_HEADER,datBuff);
00037 //    std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00038 //    delete[] datBuff;
00039 //    // 2. From that retrieved data, try to extract sub-data with the QUADROTOR_DATA_MAIN tag:
00040 //    std::vector<unsigned char> tempMainData;
00041 //    if (CAccess::extractSerializationData(developerCustomData,QUADROTOR_DATA_MAIN,tempMainData))
00042 //    { // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00043 //    }
00044 
00045 
00046 //    int buffSize=simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00047 //    char* datBuff=new char[buffSize];
00048 //    simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00049 //    std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00050 //    delete[] datBuff;
00051 
00052 //    std::vector<unsigned char> developerCustomData;
00053 //    getDeveloperCustomData(developerCustomData);
00054 
00055     // 2. From that retrieved data, try to extract sub-data with the IMU_DATA_MAIN tag:
00056 //    std::vector<unsigned char> tempMainData;
00057 
00058 //    if (CAccess::extractSerializationData(developerCustomData,IMU_DATA_FREQ,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _acquisitionFrequency:
00059 //        _acquisitionFrequency=CAccess::pop_int(tempMainData);
00060 //         std::cout << "Imu frequency: " << _acquisitionFrequency << "." << std::endl;
00061 //    } else {
00062 //        std::cout << "Imu frequency not specified. Using simulation frequency as default." << std::endl;
00063 //    }
00064 
00065 
00066     // Remove # chars for compatibility with ROS
00067     _associatedObjectName = simGetObjectName(_associatedObjectID);
00068     std::string objectName(_associatedObjectName);
00069     std::replace( objectName.begin(), objectName.end(), '#', '_');
00070     _pub = _nh.advertise<sensor_msgs::Imu>(objectName, 1000);
00071 
00072 }
00073 
00074 void ImuHandler::handleSimulation(){
00075     // called when the main script calls: simHandleModule
00076     if(!_initialized){
00077         _initialize();
00078     }
00079 
00080     ros::Time now = ros::Time::now();
00081 
00082     const simFloat currentSimulationTime = simGetSimulationTime();
00083 
00084     if ( ((currentSimulationTime - _lastPublishedImu) >= 1.0/_acquisitionFrequency) ){
00085 
00086 
00087                 Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
00088                 Eigen::Matrix<simFloat, 3, 1> angVelocity;
00089                 simFloat force[3];
00090 
00091                 if(simGetObjectQuaternion(_handleOfMass, -1, orientation.coeffs().data())!=-1 &&
00092                                 simGetObjectVelocity(_handleOfMass, NULL, angVelocity.data())!=-1 &&
00093                                 simReadForceSensor(_handleOfForceSensor, force, NULL)){
00094 
00095                         angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame
00096 
00097 /*                      // Filter force
00098                         if (_forceFilterCutoff>0){
00099                 for (uint i = 0; i<3; ++i){
00100                     double output;
00101                     _forceFilter[i]->step(std::vector<double>(1,force[i]), output);
00102                     force[i] = (simFloat)output;
00103                 }
00104                         }
00105 */
00106 
00107                         // Fill the imu msg
00108                         sensor_msgs::Imu msg;
00109 
00110                         msg.header.stamp = ros::Time::now();
00111                         msg.angular_velocity.x = angVelocity[0];
00112                         msg.angular_velocity.y = angVelocity[1];
00113                         msg.angular_velocity.z = angVelocity[2];
00114                         msg.linear_acceleration.x = force[0]/_mass;
00115                         msg.linear_acceleration.y = force[1]/_mass;
00116                         msg.linear_acceleration.z = force[2]/_mass;
00117                         _pub.publish(msg); // Publish the Imu message in ROS
00118 
00119                         _lastPublishedImu = currentSimulationTime;
00120                 }
00121 
00122     }
00123 
00124 
00125 }
00126 
00127 
00128 void ImuHandler::_initialize(){
00129     if (_initialized)
00130         return;
00131 
00132     // get some data from the main object
00133     std::vector<unsigned char> developerCustomData;
00134     getDeveloperCustomData(developerCustomData);
00135 
00136     // 2. From that retrieved data, try to extract sub-data with the IMU_DATA_MAIN tag:
00137     std::vector<unsigned char> tempMainData;
00138     std::stringstream ss;
00139 
00140     if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::IMU_DATA_FREQ,tempMainData)){
00141         _acquisitionFrequency=CAccess::pop_float(tempMainData);
00142         if (_acquisitionFrequency > 0){
00143             ss << "- [" << _associatedObjectName << "] Frequency publisher: " << _acquisitionFrequency << "." << std::endl;
00144         } else {
00145             ss << "- [" << _associatedObjectName << "] Frequency publisher: simulation frequency."  << std::endl;
00146         }
00147     } else {
00148         ss << "- [" << _associatedObjectName << "] Imu frequency publisher not specified. using simulation frequency as default."  << std::endl;
00149     }
00150 
00151 
00152     if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::IMU_DATA_CUTOFF,tempMainData)){
00153         _forceFilterCutoff=CAccess::pop_float(tempMainData);
00154         ss << "- [" << _associatedObjectName;
00155         if (_forceFilterCutoff>0){
00156             ss << "] Force will be filtered at " << _forceFilterCutoff << " Hz." << std::endl;
00157         } else {
00158             ss << "] Force filter cutoff frequency set negative. Force will not be filtered." << std::endl;
00159         }
00160     } else {
00161         _forceFilterCutoff=-1.0;
00162         ss << "- [" << _associatedObjectName << "] Force filter cutoff frequency not specified. Force will not be filtered." << std::endl;
00163     }
00164 
00165 
00166     // search associated objects
00167     std::vector<int> toExplore;
00168     toExplore.push_back(_associatedObjectID); // We start exploration with the base of the quadrotor-tree
00169     while (toExplore.size()!=0)
00170     {
00171         int objHandle=toExplore[toExplore.size()-1];
00172         toExplore.pop_back();
00173         // 1. Add this object's children to the list to explore:
00174         int index=0;
00175         int childHandle=simGetObjectChild(objHandle,index++);
00176         while (childHandle!=-1) {
00177             toExplore.push_back(childHandle);
00178 //            std::cout << "Adding " << simGetObjectName(childHandle) << " to exploration list." << std::endl;
00179             childHandle=simGetObjectChild(objHandle,index++);
00180         }
00181         // 2. Now check if this object has one of the tags we are looking for:
00182         // a. Get all the developer data attached to this scene object (this is custom data added by the developer):
00183         int buffSize=simGetObjectCustomDataLength(objHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00184         if (buffSize!=0) { // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
00185             char* datBuff=new char[buffSize];
00186             simGetObjectCustomData(objHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00187             std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00188             delete[] datBuff;
00189             // b. From that retrieved data, try to extract sub-data with the searched tags:
00190             std::vector<unsigned char> tempMainData;
00191 
00192             if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::IMU_DATA_MASS,tempMainData)){
00193                 _handleOfMass=objHandle;
00194                 simGetObjectFloatParameter(_handleOfMass, 3005, &_mass);
00195 
00196             } else if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::IMU_DATA_FORCE,tempMainData)){
00197                 _handleOfForceSensor=objHandle; // We found the IMU_DATA_FORCE tag. This is the Force sensor!
00198                 ss << "- [" << _associatedObjectName << "] Found ForceSensor. The acceleration will be computed and published." << std::endl;
00199             }
00200 
00201         }
00202     }
00203 
00204 
00205   /*  // initialize acceleration filter
00206     if(_forceFilterCutoff>0.0){
00207         const double filterSampleTime = _acquisitionFrequency > 0 ? 1.0/_acquisitionFrequency : (double)simGetSimulationTimeStep();
00208         for(uint i=0;i<3;++i){
00209             _forceFilter[i] = new telekyb::IIRFilter(telekyb::IIRLowPass(), 2.0*M_PI*_forceFilterCutoff, 1.0, filterSampleTime);
00210         }
00211     }
00212 */
00213     ConsoleHandler::printInConsole(ss);
00214 
00215     _lastPublishedImu = -1.0;
00216 
00217     _initialized=true;
00218 }
00219 
00220 bool ImuHandler::endOfSimulation(){
00221  /*   if(_forceFilterCutoff>0.0){
00222         for(uint i=0;i<3;++i){
00223             delete _forceFilter[i];
00224         }
00225     }
00226     */
00227     _initialized=false;
00228     return(false); // We don't want this object automatically destroyed at the end of simulation
00229 }
00230 
00231 PLUGINLIB_EXPORT_CLASS(ImuHandler, GenericObjectHandler)


imu_handler
Author(s): Giovanni Claudio
autogenerated on Sat Jun 8 2019 20:22:50