drawing_handler: DrawLineHandler.cpp Source File

Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <drawing_handler/DrawLineHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 #include <vrep_ros_plugin/ConsoleHandler.h>
00009 
00010 
00011 #include <boost/functional/hash.hpp>
00012 
00013 const unsigned int DrawLineHandler::DRAWING_DATA_MAIN = boost::hash<std::string>()("DrawLineHandler");
00014 const unsigned int DrawLineHandler::DRAWING_DATA_WIDTH = DrawLineHandler::DRAWING_DATA_MAIN + 1;
00015 
00016 const unsigned int DrawLineHandler::DRAWING_DATA_DIFFUSE = DrawLineHandler::DRAWING_DATA_WIDTH + 1;
00017 const unsigned int DrawLineHandler::DRAWING_DATA_SPECULAR = DrawLineHandler::DRAWING_DATA_DIFFUSE + 1;
00018 const unsigned int DrawLineHandler::DRAWING_DATA_EMISSION = DrawLineHandler::DRAWING_DATA_SPECULAR + 1;
00019 
00020 const unsigned int DrawLineHandler::DRAWING_DATA_MARKERS = DrawLineHandler::DRAWING_DATA_EMISSION + 1;
00021 const unsigned int DrawLineHandler::DRAWING_DATA_MARKERS_DIFFUSE = DrawLineHandler::DRAWING_DATA_MARKERS + 1;
00022 const unsigned int DrawLineHandler::DRAWING_DATA_MARKERS_SPECULAR = DrawLineHandler::DRAWING_DATA_MARKERS_DIFFUSE + 1;
00023 const unsigned int DrawLineHandler::DRAWING_DATA_MARKERS_EMISSION = DrawLineHandler::DRAWING_DATA_MARKERS_SPECULAR + 1;
00024 
00025 
00026 DrawLineHandler::DrawLineHandler() : GenericObjectHandler(),
00027 _lastTime(0.0),
00028 _frequency(-1),
00029 _drawingObject(-1),
00030 _width(10){
00031 
00032         for(unsigned int i=0; i<3; ++i){
00033                 _diffuse[i] = _specular[i] = _emission[i] = 0;
00034         }
00035         _diffuse[0] = 1.0;
00036 
00037         registerCustomVariables();
00038 
00039 }
00040 
00041 DrawLineHandler::~DrawLineHandler(){
00042 }
00043 
00044 unsigned int DrawLineHandler::getObjectType() const {
00045         return DRAWING_DATA_MAIN;
00046 }
00047 
00048 void DrawLineHandler::synchronize(){
00049         // We update DrawLineHandler's data from its associated scene object custom data:
00050         // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
00051         int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00052         char* datBuff=new char[buffSize];
00053         simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00054         std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00055         delete[] datBuff;
00056         // 2. From that retrieved data, try to extract sub-data with the DRAWING_DATA_MAIN tag:
00057         std::vector<unsigned char> tempMainData;
00058 
00059 
00060         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
00061 
00062                 // Remove # chars for compatibility with ROS
00063                 _associatedObjectName = simGetObjectName(_associatedObjectID);
00064                 std::stringstream streamtemp;
00065                 streamtemp << "- [Draw Line] in '" << _associatedObjectName << "'." << std::endl;
00066 
00067         }
00068 
00069 }
00070 
00071 bool DrawLineHandler::endOfSimulation(){
00072         if(_drawingObject>0){
00073                 simRemoveDrawingObject(_drawingObject);
00074                 _drawingObject = -1;
00075         }
00076         return GenericObjectHandler::endOfSimulation();
00077 }
00078 
00079 void DrawLineHandler::handleSimulation(){
00080         // called when the main script calls: simHandleModule
00081         if(!_initialized){
00082                 _initialize();
00083         }
00084 
00085         const simFloat currentSimulationTime = simGetSimulationTime();
00086 
00087         geometry_msgs::PolygonStampedConstPtr line = _line;
00088         _line.reset(); //only draw new meshes TODO: we might loose a message if it arrives between this and the previous line!
00089 
00090         if ((currentSimulationTime-_lastTime) >= 1.0/_frequency && line != NULL){
00091 
00092 
00093                 if(_drawingObject>0)
00094                         simRemoveDrawingObject(_drawingObject);
00095                 _drawingObject = simAddDrawingObject(sim_drawing_lines+sim_drawing_painttag+sim_drawing_followparentvisibility, _width, 0.0,
00096                                 _associatedObjectID, line->polygon.points.size()-1, _diffuse, NULL, _specular, _emission);
00097 
00098                 simFloat data[6];
00099                 for (geometry_msgs::PolygonStamped::_polygon_type::_points_type::const_iterator it = line->polygon.points.begin()+1;
00100                                 it!=line->polygon.points.end(); ++it){
00101                         data[0] = (it-1)->x; data[1] = (it-1)->y; data[2] = (it-1)->z;
00102                         data[3] = it->x; data[4] = it->y; data[5] = it->z;
00103                         simAddDrawingObjectItem(_drawingObject, data);
00104                 }
00105 
00106 
00107                 _lastTime = currentSimulationTime;
00108 
00109         }
00110 
00111 }
00112 
00113 void DrawLineHandler::_initialize(){
00114         if (_initialized)
00115                 return;
00116 
00117 
00118         std::vector<unsigned char> developerCustomData;
00119         getDeveloperCustomData(developerCustomData);
00120         std::vector<unsigned char> tempMainData;
00121 
00122         std::stringstream ss;
00123 
00124         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_MAIN,tempMainData)){
00125                 _frequency=CAccess::pop_float(tempMainData);
00126                 ss << "- [" << _associatedObjectName << "] Drawing frequency set to " << _frequency << "." << std::endl;
00127         } else {
00128                 _frequency = -1;
00129                 ss << "- [" << _associatedObjectName << "] Drawing frequency not specified. Using simulation step as default." << std::endl;
00130         }
00131 
00132         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_WIDTH,tempMainData)){
00133                 _width=CAccess::pop_int(tempMainData);
00134                 ss << "- [" << _associatedObjectName << "] Line width set to " << _width << "." << std::endl;
00135         } else {
00136                 _width = 1;
00137                 ss << "- [" << _associatedObjectName << "] Line width not specified. Using 1 as default" << std::endl;
00138         }
00139 
00140         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_DIFFUSE,tempMainData)){
00141                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData,3);
00142                 if (tmp.size() == 3){
00143                         memcpy(_diffuse, tmp.data(), 3*sizeof(simFloat));
00144                         ss << "- [" << _associatedObjectName << "] Diffuse color set to ["
00145                                         << _diffuse[0] << ", " << _diffuse[1] << ", " << _diffuse[2] << "]." << std::endl;
00146                 }
00147         } else {
00148                 ss << "- [" << _associatedObjectName << "] Diffuse color not specified. Using ["
00149                                 << _diffuse[0] << ", " << _diffuse[1] << ", " << _diffuse[2] << "] as default" << std::endl;
00150         }
00151 
00152         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_SPECULAR,tempMainData)){
00153                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData,3);
00154                 if (tmp.size() == 3){
00155                         memcpy(_specular, tmp.data(), 3*sizeof(simFloat));
00156                         ss << "- [" << _associatedObjectName << "] Specular color set to ["
00157                                         << _specular[0] << ", " << _specular[1] << ", " << _specular[2] << "]." << std::endl;
00158                 }
00159         } else {
00160                 ss << "- [" << _associatedObjectName << "] Specular color not specified. Using ["
00161                                 << _specular[0] << ", " << _specular[1] << ", " << _specular[2] << "] as default" << std::endl;
00162         }
00163 
00164         if (CAccess::extractSerializationData(developerCustomData,DRAWING_DATA_EMISSION,tempMainData)){
00165                 const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData,3);
00166                 if (tmp.size() == 3){
00167                         memcpy(_emission, tmp.data(), 3*sizeof(simFloat));
00168                         ss << "- [" << _associatedObjectName << "] Emission color set to ["
00169                                         << _emission[0] << ", " << _emission[1] << ", " << _emission[2] << "]." << std::endl;
00170                 }
00171         } else {
00172                 ss << "- [" << _associatedObjectName << "] Emission color not specified. Using ["
00173                                 << _emission[0] << ", " << _emission[1] << ", " << _emission[2] << "] as default" << std::endl;
00174         }
00175 
00176         std::string topicName(_associatedObjectName);
00177         std::replace( topicName.begin(), topicName.end(), '#', '_');
00178         topicName += "/DrawLine";
00179         _sub = _nh.subscribe(topicName, 1, &DrawLineHandler::lineCallback, this);
00180 
00181         ss << "- [" << _associatedObjectName << "] Waiting for line points on topic " << topicName << "." << std::endl;;
00182         ConsoleHandler::printInConsole(ss);
00183 
00184         _lastTime = -1e5;
00185         _initialized=true;
00186 }
00187 
00188 void DrawLineHandler::lineCallback(geometry_msgs::PolygonStampedConstPtr msg){
00189         _line = msg;
00190 }
00191 
00192 void DrawLineHandler::registerCustomVariables() const{
00193 #if VREP_VERSION_MAJOR*10000 + VREP_VERSION_MINOR*100 + VREP_VERSION_PATCH < 3*10000 + 3*100 + 1
00194         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_main", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MAIN))).c_str());
00195 
00196         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_width", (boost::lexical_cast<std::string>(int(DRAWING_DATA_WIDTH))).c_str());
00197 
00198         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_diffuse", (boost::lexical_cast<std::string>(int(DRAWING_DATA_DIFFUSE))).c_str());
00199         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_specular", (boost::lexical_cast<std::string>(int(DRAWING_DATA_SPECULAR))).c_str());
00200         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_emission", (boost::lexical_cast<std::string>(int(DRAWING_DATA_EMISSION))).c_str());
00201 
00202         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_markers", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS))).c_str());
00203         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_markers_diffuse", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_DIFFUSE))).c_str());
00204         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_markers_specular", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_SPECULAR))).c_str());
00205         simRegisterCustomLuaVariable("sim_ext_ros_bridge_draw_line_data_markers_emission", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_EMISSION))).c_str());
00206 #else
00207         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_main", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MAIN))).c_str(),0);
00208 
00209         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_width", (boost::lexical_cast<std::string>(int(DRAWING_DATA_WIDTH))).c_str(),0);
00210 
00211         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_diffuse", (boost::lexical_cast<std::string>(int(DRAWING_DATA_DIFFUSE))).c_str(),0);
00212         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_specular", (boost::lexical_cast<std::string>(int(DRAWING_DATA_SPECULAR))).c_str(),0);
00213         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_emission", (boost::lexical_cast<std::string>(int(DRAWING_DATA_EMISSION))).c_str(),0);
00214 
00215         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_markers", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS))).c_str(),0);
00216         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_markers_diffuse", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_DIFFUSE))).c_str(),0);
00217         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_markers_specular", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_SPECULAR))).c_str(),0);
00218         simRegisterScriptVariable("sim_ext_ros_bridge_draw_line_data_markers_emission", (boost::lexical_cast<std::string>(int(DRAWING_DATA_MARKERS_EMISSION))).c_str(),0);
00219 #endif
00220 }
00221 
00222 PLUGINLIB_EXPORT_CLASS(DrawLineHandler, GenericObjectHandler)