vrep_ros_plugin: access.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 #include "vrep_ros_plugin/access.h"
00004 #include "v_repLib.h"
00005 
00006 #include <iostream>
00007 #include <string>
00008 #include <sstream>
00009 #include <iomanip>
00010 #include <stdio.h>
00011 #include <boost/lexical_cast.hpp>
00012 
00013 CAccess::CAccess(){
00014 }
00015 
00016 CAccess::~CAccess(){
00017 }
00018 
00019 int CAccess::getDataLocationAndSize(const std::vector<unsigned char>& buffer,int dataName,int& theSize){
00020         int p=0;
00021         while (p<=int(buffer.size()-2*sizeof(int)))
00022         {
00023                 const int currHead = *((int*)(buffer.data()+p));
00024                 const int currSize = *((int*)(buffer.data()+p)+1);
00025                 if (currHead==dataName){
00026                     theSize=currSize;
00027                     return p+2*sizeof(int);
00028                 }
00029                 p+=2*sizeof(int)+currSize;
00030 //              for (int i=0;i<4;i++)
00031 //                      ((unsigned char*)&n)[i]=buffer[p+i];
00032 //              for (int i=0;i<4;i++)
00033 //                      ((unsigned char*)&l)[i]=buffer[p+4+i];
00034 //              if (n==dataName){
00035 //                      if (l!=0)
00036 //                      {
00037 //                              theSize=l;
00038 //                              return(p+8);
00039 //                      }
00040 //                      return(-1); // shouldn't happen if data saved correctly
00041 //              }
00042 //              p+=8+l;
00043         }
00044         return(-1); // data not present
00045 }
00046 
00047 bool CAccess::extractSerializationData(std::vector<unsigned char>& buffer, const int dataName, std::vector<unsigned char>& data){
00048 //      data.clear();
00049         int dataSize;
00050         const int dataLocation = getDataLocationAndSize(buffer, dataName, dataSize);
00051         if (dataLocation!=-1)
00052         {
00053 //              for (int i=0;i<l;i++)
00054 //                      data.push_back(buffer[p+i]);
00055             data.resize(dataSize);
00056             memcpy(data.data(), buffer.data()+dataLocation,dataSize);
00057                 buffer.erase(buffer.begin()+dataLocation-2*sizeof(int),buffer.begin()+dataLocation+dataSize);
00058                 return(true);
00059         }
00060         return(false); // that data was not present!
00061 }
00062 
00063 void CAccess::insertSerializationData(std::vector<unsigned char>& buffer,int dataName,const std::vector<unsigned char>& data){
00064         // 1. We remove the old data:
00065         std::vector<unsigned char> dummyBuffer;
00066 
00067         extractSerializationData(buffer,dataName,dummyBuffer);
00068         // 2. and insert the new data:
00069         // 2.1 data name:
00070 
00071         const unsigned int prevBuffSize = buffer.size();
00072         const unsigned int dataSize = data.size();
00073         buffer.resize(prevBuffSize+2*sizeof(int)+dataSize);
00074         int* prevBuffEnd = (int *)(buffer.data()+prevBuffSize);
00075 
00076         prevBuffEnd[0] = dataName;
00077         prevBuffEnd[1] = dataSize;
00078         memcpy((unsigned char *)(prevBuffEnd+2),data.data(),dataSize);
00079 
00080 //      for (int i=0;i<4;i++)
00081 //              buffer.push_back(((unsigned char*)&dataName)[i]);
00082 //
00083 //      std::cout << "bufferSize: " << buffer.size() << std::endl;
00084 //
00085 //      // 2.2 Data length:
00086 //      int l=int(data.size());
00087 //      for (int i=0;i<4;i++)
00088 //              buffer.push_back(((unsigned char*)&l)[i]);
00089 //      // 2.3 The data itself:
00090 //      for (int i=0;i<l;i++)
00091 //              buffer.push_back(data[i]);
00092 }
00093 
00094 void CAccess::push_int(std::vector<unsigned char>& buffer,int data){
00095 //      for (unsigned int i=0;i<sizeof(data);i++)
00096 //              buffer.push_back(((unsigned char*)&data)[i]);
00097         buffer.resize(buffer.size()+sizeof(int));
00098         *((int*)(&(*buffer.end()))-1) = data;
00099 }
00100 
00101 void CAccess::push_float(std::vector<unsigned char>& buffer,float data){
00102 //      for (unsigned int i=0;i<sizeof(data);i++)
00103 //              buffer.push_back(((unsigned char*)&data)[i]);
00104     buffer.resize(buffer.size()+sizeof(float));
00105     *((float*)(&(*buffer.end()))-1) = data;
00106 }
00107 
00108 int CAccess::pop_int(std::vector<unsigned char>& buffer){
00109 //      int retVal=0;
00110 //      if (buffer.size()<sizeof(retVal))
00111 //              return(retVal);
00112 //      for (int i=int(sizeof(retVal))-1;i>=0;i--)
00113 //      {
00114 //              ((unsigned char*)&retVal)[i]=buffer[buffer.size()-1];
00115 //              buffer.pop_back();
00116 //      }
00117 //      return(retVal);
00118     if (buffer.size()<sizeof(int)){
00119           return 0;
00120     } else {
00121         const float retVal=*((int*)(&(*buffer.end()))-1);
00122         buffer.resize(buffer.size()-sizeof(int));
00123         return retVal;
00124     }
00125 }
00126 
00127 float CAccess::pop_float(std::vector<unsigned char>& buffer){
00128 //      float retVal=0.0f;
00129 //      if (buffer.size()<sizeof(retVal))
00130 //              return(retVal);
00131 //      for (int i=int(sizeof(retVal))-1;i>=0;i--)
00132 //      {
00133 //              ((unsigned char*)&retVal)[i]=buffer[buffer.size()-1];
00134 //              buffer.pop_back();
00135 //      }
00136 //      return(retVal);
00137     if (buffer.size()<sizeof(float)){
00138           return 0.0;
00139     } else {
00140         const float retVal=*((float*)(&(*buffer.end()))-1);
00141         buffer.resize(buffer.size()-sizeof(float));
00142         return retVal;
00143     }
00144 }
00145 
00146 void CustomDataHeaders::registerCustomDataHeaders(){
00147 
00148 
00149     simRegisterCustomLuaVariable("sim_ext_ros_bridge_developer_data_header", (boost::lexical_cast<std::string>(int(DEVELOPER_DATA_HEADER))).c_str());
00150 
00151     // Quadrotor defines
00152     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_main", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_MAIN))).c_str());
00153     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_tk_data_main", (boost::lexical_cast<std::string>(int(QUADROTOR_TK_DATA_MAIN))).c_str());
00154     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_motor_0", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_MOTOR_0))).c_str());
00155     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_motor_1", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_MOTOR_1))).c_str());
00156     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_motor_2", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_MOTOR_2))).c_str());
00157     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_motor_3", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_MOTOR_3))).c_str());
00158     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_com", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_COM))).c_str());
00159     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_tf_ratio", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_TF_RATIO))).c_str());
00160     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_data_ctrl_mode", (boost::lexical_cast<std::string>(int(QUADROTOR_DATA_CTRL_MODE))).c_str());
00161 
00162     //Quadrotor ctrl modes
00163     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_ctrl_mode_direct", (boost::lexical_cast<std::string>(int(DIRECT))).c_str());
00164     simRegisterCustomLuaVariable("sim_ext_ros_bridge_quadrotor_ctrl_mode_internal", (boost::lexical_cast<std::string>(int(INTERNAL))).c_str());
00165 
00166     // IMU defines
00167     simRegisterCustomLuaVariable("sim_ext_ros_bridge_imu_data_main", (boost::lexical_cast<std::string>(int(IMU_DATA_MAIN))).c_str());
00168     simRegisterCustomLuaVariable("sim_ext_ros_bridge_imu_data_mass", (boost::lexical_cast<std::string>(int(IMU_DATA_MASS))).c_str());
00169     simRegisterCustomLuaVariable("sim_ext_ros_bridge_imu_data_force", (boost::lexical_cast<std::string>(int(IMU_DATA_FORCE))).c_str());
00170     simRegisterCustomLuaVariable("sim_ext_ros_bridge_imu_data_freq", (boost::lexical_cast<std::string>(int(IMU_DATA_FREQ))).c_str());
00171     simRegisterCustomLuaVariable("sim_ext_ros_bridge_imu_data_cutoff", (boost::lexical_cast<std::string>(int(IMU_DATA_CUTOFF))).c_str());
00172 
00173     // Force sensor defines
00174     simRegisterCustomLuaVariable("sim_ext_ros_bridge_force_sensor_data_main", (boost::lexical_cast<std::string>(int(FORCE_SENSOR_DATA_MAIN))).c_str());
00175 
00176     // Camera defines
00177     simRegisterCustomLuaVariable("sim_ext_ros_bridge_camera_data_main", (boost::lexical_cast<std::string>(int(CAMERA_DATA_MAIN))).c_str());
00178     simRegisterCustomLuaVariable("sim_ext_ros_bridge_camera_data_freq", (boost::lexical_cast<std::string>(int(CAMERA_DATA_FREQ))).c_str());
00179     simRegisterCustomLuaVariable("sim_ext_ros_bridge_camera_data_rgb", (boost::lexical_cast<std::string>(int(CAMERA_DATA_RGB))).c_str());
00180 
00181     // Object Pose defines
00182     simRegisterCustomLuaVariable("sim_ext_ros_bridge_obj_pose_data_main", (boost::lexical_cast<std::string>(int(OBJ_POSE_DATA_MAIN))).c_str());
00183 
00184     // Object twist defines
00185     simRegisterCustomLuaVariable("sim_ext_ros_bridge_obj_twist_data_main", (boost::lexical_cast<std::string>(int(OBJ_TWIST_DATA_MAIN))).c_str());
00186 
00187     // Set Object twist defines
00188     simRegisterCustomLuaVariable("sim_ext_ros_bridge_set_obj_twist_data_main", (boost::lexical_cast<std::string>(int(SET_OBJ_TWIST_DATA_MAIN))).c_str());
00189 
00190     // Manipulator defines
00191     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_data_main", (boost::lexical_cast<std::string>(int(MANIPULATOR_DATA_MAIN))).c_str());
00192     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_data_freq", (boost::lexical_cast<std::string>(int(MANIPULATOR_DATA_FREQ))).c_str());
00193     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_data_joint", (boost::lexical_cast<std::string>(int(MANIPULATOR_DATA_JOINT))).c_str());
00194     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_data_ctrl_mode", (boost::lexical_cast<std::string>(int(MANIPULATOR_DATA_CTRL_MODE))).c_str());
00195 
00196     // Manipulator ctrl modes
00197     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_TF_position", (boost::lexical_cast<std::string>(int(TF_POSITION))).c_str());
00198     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_TF_velocity", (boost::lexical_cast<std::string>(int(TF_VELOCITY))).c_str());
00199     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_TF_effort", (boost::lexical_cast<std::string>(int(TF_EFFORT))).c_str());
00200     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_MOT_velocity", (boost::lexical_cast<std::string>(int(MOT_VELOCITY))).c_str());
00201     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_Passive_mode", (boost::lexical_cast<std::string>(int(PASSIVE_MODE))).c_str());
00202     simRegisterCustomLuaVariable("sim_ext_ros_bridge_manipulator_ctrl_mode_Ignore_mode", (boost::lexical_cast<std::string>(int(IGNORE_MODE))).c_str());
00203 
00204     // Contact defines
00205     simRegisterCustomLuaVariable("sim_ext_ros_bridge_contact_data_main", (boost::lexical_cast<std::string>(int(CONTACT_DATA_MAIN))).c_str());
00206 
00207 }
00208 
00209 
00210 
00211