DrawMeshHandler.cpp
Go to the documentation of this file.
00001 #include <pluginlib/class_list_macros.h>
00002 
00003 #include <drawing_handler/DrawMeshHandler.h>
00004 #include <v_repLib.h>
00005 #include <vrep_ros_plugin/access.h>
00006 
00007 #include <vrep_ros_plugin/ConsoleHandler.h>
00008 
00009 #include <boost/functional/hash.hpp>
00010 
00011 const unsigned int DrawMeshHandler::DATA_MAIN = boost::hash<std::string>()("DrawMeshHandler");
00012 const unsigned int DrawMeshHandler::DATA_WIDTH = DrawMeshHandler::DATA_MAIN+1;
00013 const unsigned int DrawMeshHandler::DATA_DIFFUSE = DrawMeshHandler::DATA_WIDTH+1;
00014 const unsigned int DrawMeshHandler::DATA_SPECULAR = DrawMeshHandler::DATA_DIFFUSE+1;
00015 const unsigned int DrawMeshHandler::DATA_EMISSION = DrawMeshHandler::DATA_SPECULAR+1;
00016 const unsigned int DrawMeshHandler::DATA_TRANSPARENCY = DrawMeshHandler::DATA_EMISSION+1;
00017 
00018 DrawMeshHandler::DrawMeshHandler() :
00019                 GenericObjectHandler(), _lastTime(0.0), _frequency(-1), _drawingObject(
00020                                 -1), _width(10) {
00021 
00022         for (unsigned int i = 0; i < 3; ++i) {
00023                 _diffuse[i] = _specular[i] = _emission[i] = 0;
00024         }
00025         _diffuse[0] = 1.0;
00026 
00027         registerCustomVariables();
00028 
00029 }
00030 
00031 DrawMeshHandler::~DrawMeshHandler() {
00032 }
00033 
00034 unsigned int DrawMeshHandler::getObjectType() const {
00035         return DATA_MAIN;
00036 }
00037 
00038 void DrawMeshHandler::synchronize() {
00039         // We update DrawMeshHandler's data from its associated scene object custom data:
00040         // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00041         int buffSize = simGetObjectCustomDataLength(_associatedObjectID,
00042                         CustomDataHeaders::DEVELOPER_DATA_HEADER);
00043         char* datBuff = new char[buffSize];
00044         simGetObjectCustomData(_associatedObjectID,
00045                         CustomDataHeaders::DEVELOPER_DATA_HEADER, datBuff);
00046         std::vector<unsigned char> developerCustomData(datBuff, datBuff + buffSize);
00047         delete[] datBuff;
00048         // 2. From that retrieved data, try to extract sub-data with the DATA_MAIN tag:
00049         std::vector<unsigned char> tempMainData;
00050 
00051         if (CAccess::extractSerializationData(developerCustomData, DATA_MAIN,
00052                         tempMainData)) { // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00053 
00054                 // Remove # chars for compatibility with ROS
00055                 _associatedObjectName = simGetObjectName(_associatedObjectID);
00056                 std::stringstream streamtemp;
00057                 streamtemp << "- [Draw Mesh] in '" << _associatedObjectName << "'."
00058                                 << std::endl;
00059 
00060         }
00061 
00062 }
00063 
00064 bool DrawMeshHandler::endOfSimulation() {
00065         if (_drawingObject > 0) {
00066                 simRemoveDrawingObject(_drawingObject);
00067                 _drawingObject = -1;
00068         }
00069         return GenericObjectHandler::endOfSimulation();
00070 }
00071 
00072 void DrawMeshHandler::handleSimulation() {
00073         // called when the main script calls: simHandleModule
00074         if (!_initialized) {
00075                 _initialize();
00076         }
00077 
00078         std::stringstream ss;
00079 
00080         const simFloat currentSimulationTime = simGetSimulationTime();
00081         shape_msgs::MeshConstPtr msg = _lastMsg;
00082         _lastMsg.reset(); //only draw new meshes TODO: we might loose a message if it arrives between this and the previous line!
00083 
00084         if ((currentSimulationTime - _lastTime) >= 1.0 / _frequency && msg) {
00085 
00086                 if (_drawingObject > 0)
00087                         simRemoveDrawingObject(_drawingObject);
00088 
00089                 _drawingObject = simAddDrawingObject(
00090                                 sim_drawing_triangles + sim_drawing_painttag
00091                                                 + sim_drawing_followparentvisibility + _transparency,
00092                                 _width, 0.0, _associatedObjectID, msg->triangles.size(),
00093                                 _diffuse, NULL, _specular, _emission);
00094 
00095                 simFloat data[9];
00096                 for (shape_msgs::Mesh::_triangles_type::const_iterator it =
00097                                 msg->triangles.begin(); it != msg->triangles.end(); ++it) {
00098                         for (unsigned int i = 0; i < 3; ++i) {
00099                                 data[3 * i + 0] = msg->vertices[it->vertex_indices[i]].x;
00100                                 data[3 * i + 1] = msg->vertices[it->vertex_indices[i]].y;
00101                                 data[3 * i + 2] = msg->vertices[it->vertex_indices[i]].z;
00102                         }
00103                         simAddDrawingObjectItem(_drawingObject, data);
00104                 }
00105 
00106                 _lastTime = currentSimulationTime;
00107 
00108         }
00109 
00110         if (ss.rdbuf()->in_avail()) {
00111                 ConsoleHandler::printInConsole(ss);
00112         }
00113 }
00114 
00115 void DrawMeshHandler::_initialize() {
00116         if (_initialized)
00117                 return;
00118 
00119         std::vector<unsigned char> developerCustomData;
00120         getDeveloperCustomData(developerCustomData);
00121         std::vector<unsigned char> tempMainData;
00122 
00123         std::stringstream ss;
00124 
00125         if (CAccess::extractSerializationData(developerCustomData, DATA_MAIN,
00126                         tempMainData)) {
00127                 _frequency = CAccess::pop_float(tempMainData);
00128                 ss << "- [" << _associatedObjectName << "] Drawing frequency set to "
00129                                 << _frequency << "." << std::endl;
00130         } else {
00131                 _frequency = -1;
00132                 ss << "- [" << _associatedObjectName
00133                                 << "] Drawing frequency not specified. Using simulation step as default."
00134                                 << std::endl;
00135         }
00136 
00137         if (CAccess::extractSerializationData(developerCustomData, DATA_WIDTH,
00138                         tempMainData)) {
00139                 _width = CAccess::pop_int(tempMainData);
00140                 ss << "- [" << _associatedObjectName << "] Line width set to " << _width
00141                                 << "." << std::endl;
00142         } else {
00143                 _width = 1;
00144                 ss << "- [" << _associatedObjectName
00145                                 << "] Line width not specified. Using " << _width << " as default"
00146                                 << std::endl;
00147         }
00148 
00149         if (CAccess::extractSerializationData(developerCustomData, DATA_DIFFUSE,
00150                         tempMainData)) {
00151                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
00152                 if (tmp.size() == 3) {
00153                         memcpy(_diffuse, tmp.data(), 3 * sizeof(simFloat));
00154                         ss << "- [" << _associatedObjectName << "] Diffuse color set to ["
00155                                         << _diffuse[0] << ", " << _diffuse[1] << ", " << _diffuse[2]
00156                                         << "]." << std::endl;
00157                 }
00158         } else {
00159                 ss << "- [" << _associatedObjectName
00160                                 << "] Diffuse color not specified. Using [" << _diffuse[0]
00161                                 << ", " << _diffuse[1] << ", " << _diffuse[2] << "] as default"
00162                                 << std::endl;
00163         }
00164 
00165         if (CAccess::extractSerializationData(developerCustomData, DATA_SPECULAR,
00166                         tempMainData)) {
00167                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
00168                 if (tmp.size() == 3) {
00169                         memcpy(_specular, tmp.data(), 3 * sizeof(simFloat));
00170                         ss << "- [" << _associatedObjectName << "] Specular color set to ["
00171                                         << _specular[0] << ", " << _specular[1] << ", "
00172                                         << _specular[2] << "]." << std::endl;
00173                 }
00174         } else {
00175                 ss << "- [" << _associatedObjectName
00176                                 << "] Specular color not specified. Using [" << _specular[0]
00177                                 << ", " << _specular[1] << ", " << _specular[2]
00178                                 << "] as default" << std::endl;
00179         }
00180 
00181         if (CAccess::extractSerializationData(developerCustomData, DATA_EMISSION,
00182                         tempMainData)) {
00183                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
00184                 if (tmp.size() == 3) {
00185                         memcpy(_emission, tmp.data(), 3 * sizeof(simFloat));
00186                         ss << "- [" << _associatedObjectName << "] Emission color set to ["
00187                                         << _emission[0] << ", " << _emission[1] << ", "
00188                                         << _emission[2] << "]." << std::endl;
00189                 }
00190         } else {
00191                 ss << "- [" << _associatedObjectName
00192                                 << "] Emission color not specified. Using [" << _emission[0]
00193                                 << ", " << _emission[1] << ", " << _emission[2]
00194                                 << "] as default" << std::endl;
00195         }
00196 
00197         if (CAccess::extractSerializationData(developerCustomData,
00198                         DATA_TRANSPARENCY, tempMainData)) {
00199                 _transparency = CAccess::pop_int(tempMainData);
00200                 switch (_transparency) {
00201                 case 0:
00202                         _transparency = 0;
00203                         ss << "- [" << _associatedObjectName
00204                                         << "] Transparency set to 100%." << std::endl;
00205                         break;
00206                 case 1:
00207                         _transparency = sim_drawing_50percenttransparency;
00208                         ss << "- [" << _associatedObjectName << "] Transparency set to 50%."
00209                                         << std::endl;
00210                         break;
00211                 case 2:
00212                         _transparency = sim_drawing_25percenttransparency;
00213                         ss << "- [" << _associatedObjectName << "] Transparency set to 25%."
00214                                         << std::endl;
00215                         break;
00216                 case 3:
00217                         _transparency = sim_drawing_12percenttransparency;
00218                         ss << "- [" << _associatedObjectName
00219                                         << "] Transparency set to 12.5%." << std::endl;
00220                         break;
00221                 default:
00222                         ss << "- [" << _associatedObjectName
00223                                         << "] The specified transparency value (" << _transparency
00224                                         << ") is not valid. Transparency value can only be 0 (100%), 1 (50%), 2 (25%), or 3 (12.5). Using 100% as default."
00225                                         << std::endl;
00226                         _transparency = 0;
00227                 }
00228         } else {
00229                 _transparency = 0;
00230                 ss << "- [" << _associatedObjectName
00231                                 << "] Transparency not specified. Using 100% as default"
00232                                 << std::endl;
00233         }
00234 
00235         std::string topicName(_associatedObjectName);
00236         std::replace(topicName.begin(), topicName.end(), '#', '_');
00237         topicName += "/DrawMesh";
00238         _sub = _nh.subscribe(topicName, 1, &DrawMeshHandler::msgCallback, this);
00239 
00240         ss << "- [" << _associatedObjectName
00241                         << "] Waiting for mesh triangles on topic " << topicName << "."
00242                         << std::endl;
00243         ;
00244         ConsoleHandler::printInConsole(ss);
00245 
00246         _lastTime = -1e5;
00247         _initialized = true;
00248 }
00249 
00250 void DrawMeshHandler::msgCallback(shape_msgs::MeshConstPtr msg) {
00251         _lastMsg = msg;
00252 }
00253 
00254 void DrawMeshHandler::registerCustomVariables() const {
00255 #if VREP_VERSION_MAJOR*10000 + VREP_VERSION_MINOR*100 + VREP_VERSION_PATCH < 3*10000 + 3*100 + 1
00256         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_mesh_data_main",
00257                         (boost::lexical_cast<std::string>(int(DATA_MAIN))).c_str());
00258         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_mesh_data_width",
00259                         (boost::lexical_cast<std::string>(int(DATA_WIDTH))).c_str());
00260         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_mesh_data_diffuse",
00261                         (boost::lexical_cast<std::string>(int(DATA_DIFFUSE))).c_str());
00262         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_mesh_data_specular",
00263                         (boost::lexical_cast<std::string>(int(DATA_SPECULAR))).c_str());
00264         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_mesh_data_emission",
00265                         (boost::lexical_cast<std::string>(int(DATA_EMISSION))).c_str());
00266         simRegisterCustomLuaVariable(
00267                         "sim_ext_ros_bridge_draw_mesh_data_transparency",
00268                         (boost::lexical_cast<std::string>(int(DATA_TRANSPARENCY))).c_str());
00269 #else
00270         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_main",
00271                         (boost::lexical_cast<std::string>(int(DATA_MAIN))).c_str(),0);
00272         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_width",
00273                         (boost::lexical_cast<std::string>(int(DATA_WIDTH))).c_str(),0);
00274         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_diffuse",
00275                         (boost::lexical_cast<std::string>(int(DATA_DIFFUSE))).c_str(),0);
00276         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_specular",
00277                         (boost::lexical_cast<std::string>(int(DATA_SPECULAR))).c_str(),0);
00278         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_emission",
00279                         (boost::lexical_cast<std::string>(int(DATA_EMISSION))).c_str(),0);
00280         simRegisterScriptVariable("sim_ext_ros_bridge_draw_mesh_data_transparency",
00281                         (boost::lexical_cast<std::string>(int(DATA_TRANSPARENCY))).c_str(),0);
00282 #endif  
00283 }
00284 
00285 PLUGINLIB_EXPORT_CLASS(DrawMeshHandler, GenericObjectHandler)


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