├── .gitignore ├── README.md ├── pacpussensors ├── StdDbtPlayerComponents │ ├── .svn │ │ ├── all-wcprops │ │ ├── dir-prop-base │ │ ├── entries │ │ └── text-base │ │ │ ├── CMakeLists.txt.svn-base │ │ │ ├── DbtPlyImageManager.cpp.svn-base │ │ │ ├── DbtPlyImageManager.h.svn-base │ │ │ ├── DbtPlyStereoManager.cpp.svn-base │ │ │ ├── DbtPlyStereoManager.h.svn-base │ │ │ ├── DbtPlyVisionConfig.h.svn-base │ │ │ ├── ImageViewer.cpp.svn-base │ │ │ ├── ImageViewer.h.svn-base │ │ │ └── StdDbtPlayerComponentsConfig.h.svn-base │ ├── CMakeLists.txt │ ├── DbtPlyImageManager.cpp │ ├── DbtPlyImageManager.h │ ├── DbtPlyStereoManager.cpp │ ├── DbtPlyStereoManager.h │ ├── DbtPlyVelodyneManager.cpp │ ├── DbtPlyVelodyneManager.h │ ├── DbtPlyVisionConfig.h │ ├── ImageViewer.cpp │ ├── ImageViewer.h │ ├── StdDbtPlayerComponentsConfig.h │ ├── StdDbtPlayerComponentsPlugin.cpp │ └── StdDbtPlayerComponentsPlugin.h ├── VelodyneComponent │ ├── --CMakeLists.txt │ ├── .svn │ │ ├── all-wcprops │ │ ├── dir-prop-base │ │ ├── entries │ │ └── text-base │ │ │ ├── CMakeLists.txt.svn-base │ │ │ ├── VelodyneComponent.cpp.svn-base │ │ │ ├── VelodyneComponent.h.svn-base │ │ │ └── VelodyneViewer.h.svn-base │ ├── CMakeLists.txt │ ├── VelodyneComponent.cpp │ ├── VelodyneComponent.h │ ├── VelodyneComponentPlugin.cpp │ ├── VelodyneComponentPlugin.h │ ├── VelodyneHDL64S2Plugin.cpp │ ├── VelodyneHDL64S2Plugin.h │ ├── VelodyneViewer.h │ └── structure_velodyne.h └── tx_p12 │ ├── .svn │ ├── entries │ ├── format │ ├── pristine │ │ ├── 36 │ │ │ └── 367630f4a6f38b7077d89e4e9b8ce507b164e3db.svn-base │ │ ├── 58 │ │ │ └── 58a1159d9132ad287eca14df70e47f43f7326b77.svn-base │ │ ├── 84 │ │ │ └── 84b80d7aa2724400f8602f142a597807c14c0495.svn-base │ │ ├── 7f │ │ │ └── 7f44c14ec58f418bfb2e8542f9033633faafbfac.svn-base │ │ ├── 8c │ │ │ └── 8cb5e7b83ff392a3e5162338371b361605e65bcc.svn-base │ │ ├── ac │ │ │ └── ac3ad1c9c4284a4c3969468c1b7d5660d4c42b4b.svn-base │ │ ├── bb │ │ │ └── bbca14642c5caa2d6601aebd1e5688737cde6f83.svn-base │ │ ├── d3 │ │ │ └── d32074f3a4a5ab6de4d8491a3cf2bdf182fdceed.svn-base │ │ └── f3 │ │ │ └── f3594cc2a482dcae92be813ce592349a5e4b1d03.svn-base │ └── wc.db │ ├── CMakeLists.txt │ ├── ComputingComponent.cpp │ ├── ComputingComponent.h │ ├── VelodyneInterface.cpp │ ├── VelodyneInterface.h │ ├── VelodynePCLViewerPlugin.cpp │ ├── VelodynePCLViewerPlugin.h │ ├── XML │ ├── VelodynePCLViewer.xml │ └── VelodynePCLViewer_d.xml │ ├── main.cpp │ ├── structure_velodyne_cart.h │ └── ui │ ├── widgetPCL.cpp │ └── widgetPCL.h ├── rapport.pdf ├── rapport.tex ├── tests ├── bunny.pcd ├── input.pcd ├── test_floor_detection.cpp ├── test_meshing.cpp └── test_poisson.cpp └── xml ├── compute.xml ├── conf.xml ├── db.xml └── velodyne.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *.dbt 2 | CMakeCache.txt 3 | CMakeFiles 4 | Makefile 5 | cmake_install.cmake 6 | install_manifest.txt -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #Instructions to use it without Pacpus 2 | 3 | Install PCL 4 | Compile & run any of the test_*.cpp file linked with PCL 5 | 6 | #Instructions to use it with Pacpus 7 | 8 | ##Compile & Install Pacpus 9 | https://devel.hds.utc.fr/svn/pacpusframework/ 10 | https://devel.hds.utc.fr/svn/pacpusbase/ 11 | https://devel.hds.utc.fr/svn/pacpussensors/ 12 | 13 | See instructions on the wiki because things are moving at the moment 14 | 15 | ##Compile && PacpusSensors 16 | 17 | export PACPUS_ROOT="/opt/pacpus/0.0.1" 18 | cd pacpus/pacpussensors/trunk/build/ 19 | ./build_linux.sh 20 | 21 | Add files in this repository to get the last working version of PacpusSensors with Velodyne 22 | 23 | ##Data player 24 | 25 | PATH=$PATH:/opt/pacpus/0.0.1/bin 26 | cd pacpus/ 27 | DBITEPlayer_d xml/conf.xml 28 | 29 | ##Data viewer 30 | 31 | PATH=$PATH:/opt/pacpus/0.0.1/bin 32 | cd pacpus/ 33 | PacpusSensor_d xml/compute.xml 34 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 59 4 | /svn/pacpussensors/!svn/ver/20/trunk/StdDbtPlayerComponents 5 | END 6 | ImageViewer.h 7 | K 25 8 | svn:wc:ra_dav:version-url 9 | V 73 10 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/ImageViewer.h 11 | END 12 | DbtPlyStereoManager.cpp 13 | K 25 14 | svn:wc:ra_dav:version-url 15 | V 83 16 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/DbtPlyStereoManager.cpp 17 | END 18 | CMakeLists.txt 19 | K 25 20 | svn:wc:ra_dav:version-url 21 | V 74 22 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/CMakeLists.txt 23 | END 24 | DbtPlyImageManager.cpp 25 | K 25 26 | svn:wc:ra_dav:version-url 27 | V 82 28 | /svn/pacpussensors/!svn/ver/18/trunk/StdDbtPlayerComponents/DbtPlyImageManager.cpp 29 | END 30 | DbtPlyStereoManager.h 31 | K 25 32 | svn:wc:ra_dav:version-url 33 | V 81 34 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/DbtPlyStereoManager.h 35 | END 36 | DbtPlyImageManager.h 37 | K 25 38 | svn:wc:ra_dav:version-url 39 | V 80 40 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/DbtPlyImageManager.h 41 | END 42 | DbtPlyVisionConfig.h 43 | K 25 44 | svn:wc:ra_dav:version-url 45 | V 80 46 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/DbtPlyVisionConfig.h 47 | END 48 | StdDbtPlayerComponentsConfig.h 49 | K 25 50 | svn:wc:ra_dav:version-url 51 | V 90 52 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/StdDbtPlayerComponentsConfig.h 53 | END 54 | ImageViewer.cpp 55 | K 25 56 | svn:wc:ra_dav:version-url 57 | V 75 58 | /svn/pacpussensors/!svn/ver/16/trunk/StdDbtPlayerComponents/ImageViewer.cpp 59 | END 60 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/dir-prop-base: -------------------------------------------------------------------------------- 1 | K 10 2 | svn:ignore 3 | V 35 4 | *Plugin.cpp 5 | *Plugin.h 6 | *Plugin.json 7 | 8 | END 9 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 26 5 | https://devel.hds.utc.fr/svn/pacpussensors/trunk/StdDbtPlayerComponents 6 | https://devel.hds.utc.fr/svn/pacpussensors 7 | 8 | 9 | 10 | 2013-07-31T09:59:48.889571Z 11 | 20 12 | kurdejma 13 | has-props 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 224c60b7-45b5-497f-b0fe-5b4e42807f72 28 | 29 | DbtPlyStereoManager.h 30 | file 31 | 32 | 33 | 34 | 35 | 2013-10-22T12:32:06.220702Z 36 | 5702dd96ee1cb7b2db920aa21967672a 37 | 2013-07-18T10:09:40.179168Z 38 | 16 39 | gdherbom 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 1705 62 | 63 | DbtPlyImageManager.h 64 | file 65 | 66 | 67 | 68 | 69 | 2013-10-22T12:32:06.220702Z 70 | daeed6ffa16dbdf18277867d3249eeb3 71 | 2013-07-18T10:09:40.179168Z 72 | 16 73 | gdherbom 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 1553 96 | 97 | DbtPlyVisionConfig.h 98 | file 99 | 100 | 101 | 102 | 103 | 2013-10-22T12:32:06.220702Z 104 | 5cad3d2a17aaeebb01512386378982a7 105 | 2013-07-18T10:09:40.179168Z 106 | 16 107 | gdherbom 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 477 130 | 131 | StdDbtPlayerComponentsConfig.h 132 | file 133 | 134 | 135 | 136 | 137 | 2013-10-22T12:32:06.220702Z 138 | f01aa6d594eed3afac1c0b1c09e9c576 139 | 2013-07-18T10:09:40.179168Z 140 | 16 141 | gdherbom 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 493 164 | 165 | ImageViewer.cpp 166 | file 167 | 168 | 169 | 170 | 171 | 2013-10-22T12:32:06.216702Z 172 | 8526fd8f053cfa81f6b27b4ec77c2037 173 | 2013-07-18T10:09:40.179168Z 174 | 16 175 | gdherbom 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 2866 198 | 199 | ImageViewer.h 200 | file 201 | 202 | 203 | 204 | 205 | 2013-10-22T12:32:06.216702Z 206 | 55b3786645f224f2d0738bd19df2abd7 207 | 2013-07-18T10:09:40.179168Z 208 | 16 209 | gdherbom 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 1162 232 | 233 | DbtPlyStereoManager.cpp 234 | file 235 | 236 | 237 | 238 | 239 | 2013-10-22T12:32:06.220702Z 240 | 16e8d99d97dd287fb17f7b203cebc94e 241 | 2013-07-18T10:09:40.179168Z 242 | 16 243 | gdherbom 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 4505 266 | 267 | CMakeLists.txt 268 | file 269 | 270 | 271 | 272 | 273 | 2013-10-22T12:32:06.220702Z 274 | 40dcf17752e5041a839643c68e2f8f8f 275 | 2013-07-18T10:09:40.179168Z 276 | 16 277 | gdherbom 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 2692 300 | 301 | DbtPlyImageManager.cpp 302 | file 303 | 304 | 305 | 306 | 307 | 2013-10-22T12:32:06.220702Z 308 | 5caa71951aa069f044ea4f1dadcc2d65 309 | 2013-07-31T09:57:02.146098Z 310 | 18 311 | kurdejma 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 6027 334 | 335 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/CMakeLists.txt.svn-base: -------------------------------------------------------------------------------- 1 | project(StdDbtPlayerComponents) 2 | 3 | ################################################################################ 4 | add_definitions( -DSTDDBTPLAYERCOMPONENTS_EXPORTS ) 5 | 6 | # ======================================== 7 | # Configure qt4 8 | # ======================================== 9 | if(QT4_FOUND) 10 | set(QT_USE_QTXML true) 11 | set(QT_USE_QTNETWORK true) 12 | include(${QT_USE_FILE}) 13 | else() 14 | message(ERROR "Qt4 needed") 15 | endif() 16 | 17 | # ======================================== 18 | # Compiler definitions 19 | # ======================================== 20 | add_definitions( 21 | ${QT_DEFINITIONS} 22 | ) 23 | 24 | # ======================================== 25 | # Include directories 26 | # ======================================== 27 | include_directories( 28 | ${PROJECT_BINARY_DIR} 29 | ${QT_INCLUDE_DIR} 30 | ) 31 | 32 | # ======================================== 33 | # Link directories 34 | # ======================================== 35 | link_directories( ${PACPUS_LIB_DIR} 36 | ) 37 | 38 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME} ) 39 | 40 | set(HDRS 41 | DbtPlyImageManager.h 42 | DbtPlyStereoManager.h 43 | ImageViewer.h 44 | StdDbtPlayerComponentsConfig.h 45 | ) 46 | 47 | 48 | # ======================================== 49 | # List of sources 50 | # ======================================== 51 | set( 52 | PROJECT_SRCS 53 | DbtPlyImageManager.cpp 54 | DbtPlyStereoManager.cpp 55 | ImageViewer.cpp 56 | ${HDRS} 57 | ${PLUGIN_CPP} 58 | ) 59 | 60 | # ======================================== 61 | # Files to MOC 62 | # ======================================== 63 | set( 64 | FILES_TO_MOC 65 | DbtPlyImageManager.h 66 | DbtPlyStereoManager.h 67 | ImageViewer.h 68 | ${PLUGIN_H} 69 | ) 70 | 71 | 72 | set( 73 | UI_FILES 74 | 75 | ) 76 | 77 | # ======================================== 78 | # Call MOC 79 | # ======================================== 80 | qt4_wrap_cpp( 81 | PROJECT_MOC_SRCS 82 | ${FILES_TO_MOC} 83 | ) 84 | 85 | qt4_wrap_ui( 86 | PROJECT_UI_SRCS 87 | ${UI_FILES} 88 | ) 89 | 90 | # ======================================== 91 | # Build a library 92 | # ======================================== 93 | pacpus_add_library( 94 | ${PROJECT_NAME} SHARED 95 | ${PROJECT_SRCS} 96 | ${PROJECT_MOC_SRCS} 97 | ${PROJECT_UI_SRCS} 98 | ) 99 | 100 | set(LIBS 101 | optimized FileLib debug FileLib_d 102 | optimized PacpusLib debug PacpusLib_d 103 | optimized PacpusTools debug PacpusTools_d 104 | ) 105 | if (WIN32) 106 | list(APPEND LIBS 107 | optimized ROAD_TIME debug ROAD_TIME_d 108 | ) 109 | endif() 110 | 111 | # ======================================== 112 | # Libraries 113 | # ======================================== 114 | # All the platform 115 | target_link_libraries( 116 | ${PROJECT_NAME} 117 | ${PACPUS_LIBRARIES} 118 | ${QT_LIBRARIES} 119 | ${PACPUS_DEPENDENCIES_LIB} 120 | ${LIBS} 121 | ) 122 | 123 | pacpus_folder(${PROJECT_NAME} "components") 124 | 125 | # ======================================== 126 | # Install 127 | # ======================================== 128 | pacpus_install(${PROJECT_NAME}) 129 | 130 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/DbtPlyImageManager.cpp.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2007/04/12 - 16:30 3 | // 4 | // author: Elie Al Alam & Gerald Dherbomez 5 | // 6 | // version: $Id: DbtPlyImageManager.cpp 1239 2012-11-28 16:30:00Z kurdejma $ 7 | // 8 | // purpose: Dbite Player Image Manager implementation 9 | *********************************************************************/ 10 | 11 | #include "DbtPlyImageManager.h" 12 | 13 | #include "ImageViewer.h" 14 | 15 | #include "kernel/ComponentFactory.h" 16 | #include "kernel/ComponentManager.h" 17 | #include "kernel/DbiteFileTypes.h" 18 | #include "kernel/Log.h" 19 | #include "PacpusTools/ShMem.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace pacpus { 26 | 27 | using namespace std; 28 | 29 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyImageManager"); 30 | 31 | static const string kDefaultMemoryName = "IMAGE"; 32 | 33 | /// Construction de la fabrique de composant DbtPlyImageManager 34 | static ComponentFactory sFactory("DbtPlyImageManager"); 35 | 36 | /// Constructor 37 | DbtPlyImageManager::DbtPlyImageManager(QString name) 38 | : DbtPlyFileManager (name) 39 | , im_(NULL) 40 | , shMem_(NULL) 41 | , imageMutex_(new QMutex()) 42 | , firstTime(true) 43 | { 44 | } 45 | 46 | /// Destructor 47 | DbtPlyImageManager::~DbtPlyImageManager() 48 | { 49 | // #if WIN32 50 | delete imageMutex_; 51 | // #endif 52 | if (shMem_) { 53 | delete shMem_; 54 | shMem_ = NULL; 55 | } 56 | } 57 | 58 | /// TODO: doc 59 | void DbtPlyImageManager::processData(road_time_t /*t*/, road_timerange_t /*tr*/, void * buf) 60 | { 61 | if (!buf) { 62 | LOG_WARN("no data to process: empty data buffer"); 63 | return; 64 | } 65 | 66 | // look at the dbt index in file manager and get the identifier of dbt structure 67 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType(); 68 | 69 | switch (id) { 70 | case STEREO_LEFT_IMAGE: 71 | case STEREO_RIGHT_IMAGE: 72 | //{ 73 | // QMutexLocker mutexLocker(imageMutex_); 74 | // Q_UNUSED(mutexLocker); 75 | // if (firstTime) { 76 | // im_ = new QImage(320, 240, QImage::Format_RGB32); 77 | // if (!im_) { 78 | // LOG_ERROR("invalid image"); 79 | // } 80 | // shMem_ = new ShMem(kDefaultMemoryName.c_str(), im_->numBytes()); 81 | // firstTime = false; 82 | // } 83 | // // convert image to 32 bits for the display 84 | // YtoRGB32(im_->bits(), (unsigned char *)buf); 85 | // shMem_->write(im_->bits(), im_->numBytes()); 86 | //} 87 | // break; 88 | 89 | case FILE_JPEG: 90 | imageFile_ = mDbtDataPath + (char *)buf; 91 | LOG_TRACE("image path = " << imageFile_); 92 | if (mVerbose) { 93 | cout << "[IMAGE]:\t" 94 | << imageFile_.toStdString() << endl 95 | ; 96 | } 97 | 98 | { 99 | QMutexLocker mutexLocker(imageMutex_); 100 | Q_UNUSED(mutexLocker); 101 | if (!im_) { 102 | LOG_TRACE("allocating image"); 103 | im_ = new QImage(); 104 | if (!im_) { 105 | LOG_ERROR("cannot allocate image"); 106 | } 107 | } 108 | static int shmemSize; 109 | bool imageLoaded = im_->load(imageFile_); 110 | if (!imageLoaded) { 111 | LOG_WARN("cannot load image file '" << imageFile_ << "'"); 112 | return; 113 | } 114 | LOG_TRACE("image loaded"); 115 | if (firstTime) { 116 | // send image in shared memory 117 | shmemSize = im_->byteCount(); 118 | LOG_TRACE("'" << mShMemName << "'" << " memory size = " << shmemSize); 119 | shMem_ = new ShMem(mShMemName.toStdString().c_str(), shmemSize); 120 | firstTime = false; 121 | } 122 | shMem_->write(im_->bits(), shmemSize); 123 | } 124 | break; 125 | 126 | default: 127 | break; 128 | } 129 | 130 | Q_EMIT displayIm(im_); 131 | } 132 | 133 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyImageManager::configureComponent(XmlComponentConfig config) 134 | { 135 | mShMemName = kDefaultMemoryName.c_str(); 136 | if (param.getProperty("shmem") != QString::null) { 137 | mShMemName = param.getProperty("shmem"); 138 | } 139 | return DbtPlyFileManager::configureComponent(config); 140 | } 141 | 142 | void DbtPlyImageManager::startActivity() 143 | { 144 | DbtPlyFileManager::startActivity(); 145 | } 146 | 147 | void DbtPlyImageManager::stopActivity() 148 | { 149 | delete im_; im_ = NULL; 150 | DbtPlyFileManager::stopActivity(); 151 | } 152 | 153 | //////////////////////////////////////////////////////////////////////////////// 154 | /// Pour l'affichage de l'image 155 | void DbtPlyImageManager::displayUI() 156 | { 157 | imviewer_ = new ImageViewer; 158 | imviewer_->setMutex(imageMutex_); 159 | imviewer_->show(); 160 | imviewer_->setWindowTitle(componentName); 161 | connect(this, SIGNAL(displayIm(QImage*)), imviewer_, SLOT(display(QImage*))); 162 | } 163 | 164 | //////////////////////////////////////////////////////////////////////////////// 165 | /// Convert the image from Y to RGB32 166 | /// The image is stored using a 32-bit RGB format (0xffRRGGBB) 167 | void DbtPlyImageManager::YtoRGB32(unsigned char * dest, unsigned char * src) 168 | { 169 | unsigned char *srcptr, *srcend, *destptr; 170 | 171 | srcptr = src; 172 | srcend = srcptr + (320 * 240); 173 | destptr = dest; 174 | 175 | // single-stage idiotproofing 176 | if (src == NULL || dest == NULL) 177 | return; 178 | 179 | // just Y's (monochrome) 180 | 181 | // unroll it to 4 per cycle 182 | 183 | while(srcptr < srcend) { 184 | *destptr++ = *srcptr; 185 | *destptr++ = *srcptr; 186 | *destptr++ = *srcptr; 187 | destptr++;//A 188 | srcptr++; 189 | 190 | *destptr++ = *srcptr; 191 | *destptr++ = *srcptr; 192 | *destptr++ = *srcptr; 193 | destptr++;//A 194 | srcptr++; 195 | 196 | *destptr++ = *srcptr; 197 | *destptr++ = *srcptr; 198 | *destptr++ = *srcptr; 199 | destptr++;//A 200 | srcptr++; 201 | 202 | *destptr++ = *srcptr; 203 | *destptr++ = *srcptr; 204 | *destptr++ = *srcptr; 205 | destptr++;//A 206 | srcptr++; 207 | } 208 | } 209 | 210 | void DbtPlyImageManager::tic() 211 | { 212 | tic_ = road_time(); 213 | } 214 | 215 | void DbtPlyImageManager::toc(char * text) 216 | { 217 | cout << "duration = " << (int)(road_time() - tic_) << "\t" 218 | << text << "\n" 219 | ; 220 | } 221 | 222 | } // namespace pacpus 223 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/DbtPlyImageManager.h.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2007/04/12 - 16:30 3 | 4 | // 5 | // author: Elie Al Alam & Gerald Dherbomez 6 | // 7 | // version: $Id: $ 8 | // 9 | // purpose: Dbite Player Image Manager header file 10 | *********************************************************************/ 11 | 12 | #ifndef DBTPLYIMAGEMANAGER_H 13 | #define DBTPLYIMAGEMANAGER_H 14 | 15 | #include 16 | 17 | #include "StdDbtPlayerComponentsConfig.h" 18 | #include "DbitePlayer/DbtPlyFileManager.h" 19 | 20 | class ImageViewer; 21 | 22 | class QImage; 23 | class QMutex; 24 | 25 | namespace pacpus { 26 | 27 | class ShMem; 28 | 29 | /// 30 | /// shmem=STRING (name of output shared memory) default=IMAGE 31 | class STDDBTPLAYERCOMPONENTS_API DbtPlyImageManager 32 | : public DbtPlyFileManager 33 | { 34 | Q_OBJECT 35 | 36 | public: 37 | DbtPlyImageManager(QString name); 38 | ~DbtPlyImageManager(); 39 | 40 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 41 | 42 | void displayUI(); 43 | 44 | private: 45 | QString mShMemName; 46 | QString imageFile_; 47 | QImage * im_; 48 | 49 | ImageViewer * imviewer_; 50 | 51 | protected: 52 | void processData(road_time_t, road_timerange_t, void * dataBuffer); 53 | virtual void startActivity(); 54 | virtual void stopActivity(); 55 | 56 | Q_SIGNALS: 57 | void displayIm(QImage *); 58 | 59 | private: 60 | QMutex * imageMutex_; 61 | ShMem * shMem_; 62 | bool firstTime; 63 | 64 | road_time_t tic_; 65 | void tic(); 66 | void toc(char * text); 67 | 68 | void YtoRGB32(unsigned char * dest, unsigned char * src); 69 | }; 70 | 71 | } // namespace pacpus 72 | 73 | #endif // DBTPLYIMAGEMANAGER_H 74 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/DbtPlyStereoManager.cpp.svn-base: -------------------------------------------------------------------------------- 1 | /// purpose: Dbite Player Stereo Manager implementation 2 | /// 3 | /// created @date 2008/01/19 - 21:23 4 | /// @author Sergio Rodriguez 5 | /// @version $Id: $ 6 | 7 | #include "DbtPlyStereoManager.h" 8 | 9 | #include 10 | 11 | #include "kernel/ComponentManager.h" 12 | #include "kernel/DbiteFileTypes.h" 13 | #include "kernel/Log.h" 14 | 15 | namespace pacpus { 16 | 17 | using namespace std; 18 | 19 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyStereoManager"); 20 | 21 | /// Construction de la fabrique de composant DbtPlyStereoManager 22 | static ComponentFactory sFactory("DbtPlyStereoManager"); 23 | 24 | DbtPlyStereoManager::DbtPlyStereoManager(QString name) 25 | : DbtPlyFileManager(name) 26 | { 27 | firstTime = true; 28 | tobeRead = NULL; 29 | 30 | left0 = NULL; 31 | right0 = NULL; 32 | timeStamp0 = NULL; 33 | timeRange0 = NULL; 34 | 35 | left1 = NULL; 36 | right1 = NULL; 37 | timeStamp1 = NULL; 38 | timeRange1 = NULL; 39 | } 40 | 41 | DbtPlyStereoManager::~DbtPlyStereoManager() 42 | { 43 | delete tobeRead; 44 | delete left0; 45 | delete right0; 46 | delete timeStamp0; 47 | delete timeRange0; 48 | delete left1; 49 | delete right1; 50 | delete timeStamp1; 51 | delete timeRange1; 52 | } 53 | 54 | //processData unix 55 | void DbtPlyStereoManager::processData(road_time_t t, road_timerange_t tr, void * buf) 56 | { 57 | if (!buf) { 58 | return; 59 | } 60 | 61 | const size_t imageSize = width_ * height_ * depth_; 62 | 63 | // look at the dbt index in file manager and get the identifier of dbt structure 64 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType(); 65 | 66 | if (firstTime) { 67 | tobeRead = new ShMem("OFFSET", sizeof(bool)); //Offset pointer to read 68 | 69 | left0 = new ShMem("BufferLeft0", imageSize); 70 | right0 = new ShMem("BufferRight0", imageSize); 71 | timeStamp0 = new ShMem("BufferTimeStamp0", sizeof(road_time_t)); 72 | timeRange0 = new ShMem("BufferTimeRange0", sizeof(road_timerange_t)); 73 | 74 | left1 = new ShMem("BufferLeft1", imageSize); 75 | right1 = new ShMem("BufferRight1", imageSize); 76 | timeStamp1 = new ShMem("BufferTimeStamp1", sizeof(road_time_t)); 77 | timeRange1 = new ShMem("BufferTimeRange1", sizeof(road_timerange_t)); 78 | 79 | firstTime = false; 80 | leftStatus = false; 81 | rightStatus = false; 82 | offset = false; 83 | 84 | if (tobeRead == NULL) { 85 | LOG_FATAL("Crashing because of shared memory"); 86 | } 87 | } 88 | 89 | //dropping stereo images 90 | switch (id) { 91 | case STEREO_LEFT_IMAGE: 92 | if (!offset) 93 | left0->write(buf, imageSize); 94 | else 95 | left1->write(buf, imageSize); 96 | leftStatus = true; 97 | break; 98 | case STEREO_RIGHT_IMAGE: 99 | if (!offset) 100 | right0->write(buf, imageSize); 101 | else 102 | right1->write(buf, imageSize); 103 | rightStatus = true; 104 | break; 105 | default: 106 | //Unkown id data type 107 | break; 108 | } 109 | 110 | if ((leftStatus)&&(rightStatus)) { // synchronized stereo data is available 111 | //dropping time stamp 112 | if (!offset) { 113 | timeStamp0->write(&t, sizeof(road_time_t)); 114 | timeRange0->write(&tr, sizeof(road_timerange_t)); 115 | } else { 116 | timeStamp1->write(&t, sizeof(road_time_t)); 117 | timeRange1->write(&tr, sizeof(road_timerange_t)); 118 | } 119 | if (mVerbose >= 1) { 120 | cout << "[Shmem update Event] at " << t << std::endl; 121 | } 122 | //setting read flag 123 | tobeRead->write((char*)&offset, sizeof(bool)); 124 | if (!offset) 125 | offset = true; 126 | else 127 | offset =false; 128 | 129 | leftStatus = false; 130 | rightStatus = false; 131 | } 132 | } 133 | 134 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyStereoManager::configureComponent(XmlComponentConfig config) 135 | { 136 | //default stereo image parameters 137 | width_ = 320; // pixels 138 | height_ = 240; // pixels 139 | depth_ = 1; // bytes-per-pixel 140 | 141 | //load stereo image parameters 142 | width_ = param.getProperty("width").toInt(); 143 | height_ = param.getProperty("height").toInt(); 144 | depth_ = param.getProperty("depth").toInt(); 145 | 146 | return DbtPlyFileManager::configureComponent(config); 147 | } 148 | 149 | void DbtPlyStereoManager::startActivity() 150 | { 151 | DbtPlyFileManager::startActivity(); 152 | } 153 | 154 | void DbtPlyStereoManager::stopActivity() 155 | { 156 | DbtPlyFileManager::stopActivity(); 157 | } 158 | 159 | void DbtPlyStereoManager::displayUI() 160 | { 161 | // TODO: not implemented 162 | } 163 | 164 | void DbtPlyStereoManager::tic() 165 | { 166 | tic_ = road_time(); 167 | } 168 | 169 | void DbtPlyStereoManager::toc(char * text) 170 | { 171 | cout << "duration = " << (int)(road_time() - tic_) << " " << text << "\n"; 172 | } 173 | 174 | } // namespace pacpus 175 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/DbtPlyStereoManager.h.svn-base: -------------------------------------------------------------------------------- 1 | /// purpose: Dbite Player Stereo Manager header file 2 | /// 3 | /// created @date 2008/01/19 - 21:23 4 | /// @author Sergio Rodriguez 5 | /// @version $Id: $ 6 | 7 | #ifndef DBTPLYSTEREOMANAGER_H 8 | #define DBTPLYSTEREOMANAGER_H 9 | 10 | #include "StdDbtPlayerComponentsConfig.h" 11 | #include "kernel/ComponentBase.h" 12 | #include "kernel/ComponentFactory.h" 13 | #include "DbitePlayer/DbtPlyFileManager.h" 14 | #include "PacpusTools/ShMem.h" 15 | 16 | namespace pacpus { 17 | 18 | class STDDBTPLAYERCOMPONENTS_API DbtPlyStereoManager 19 | : public DbtPlyFileManager 20 | { 21 | Q_OBJECT 22 | 23 | public: 24 | DbtPlyStereoManager(QString name); 25 | ~DbtPlyStereoManager(); 26 | 27 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 28 | void displayUI(); 29 | 30 | protected: 31 | void processData(road_time_t t, road_timerange_t tr, void * buffer); 32 | virtual void startActivity(); 33 | virtual void stopActivity(); 34 | 35 | private: 36 | ShMem * tobeRead; //Buffer flag 37 | 38 | ShMem * left0; //left image of Buffer 0 39 | ShMem * right0; //right image of Buffer 0 40 | ShMem * timeStamp0; //Timestamp of stereo images in Buffer 0 41 | ShMem * timeRange0; //Uncertainty of timeStamp Buffer 0 42 | 43 | ShMem * left1; //left image of Buffer 1 44 | ShMem * right1; //right image of Buffer 1 45 | ShMem * timeStamp1; //Timestamp of stereo images in Buffer 1 46 | ShMem * timeRange1; //Uncertainty of timeStamp Buffer 1 47 | //#endif 48 | 49 | bool firstTime; 50 | bool offset; 51 | bool leftStatus; 52 | bool rightStatus; 53 | //image paramaters 54 | int width_; 55 | int height_; 56 | int depth_; 57 | 58 | road_time_t tic_; 59 | void tic(); 60 | void toc(char * text); 61 | }; 62 | 63 | } // namespace pacpus 64 | 65 | #endif // DBTPLYSTEREOMANAGER_H 66 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/DbtPlyVisionConfig.h.svn-base: -------------------------------------------------------------------------------- 1 | #ifndef DBTPLYVISIONCONFIG_H 2 | #define DBTPLYVISIONCONFIG_H 3 | 4 | // Export macro for DbtPlyVision DLL for Windows only 5 | #ifdef WIN32 6 | # if defined(DBTPLYVISION_EXPORTS) || defined(STEREO_EXPORTS) 7 | // make DLL 8 | # define DBTPLYVISION_API __declspec(dllexport) 9 | # else 10 | // use DLL 11 | # define DBTPLYVISION_API __declspec(dllimport) 12 | # endif 13 | #else 14 | // On other platforms, simply ignore this 15 | # define DBTPLYVISION_API 16 | #endif 17 | 18 | #endif // DBTPLYVISIONCONFIG_H 19 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/ImageViewer.cpp.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2006/06/09 - 9:40 3 | // filename: ImageViewer.cpp 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id$ 9 | // 10 | // purpose: Definition of the ImageViewer class 11 | // 12 | // todo : - think how to display geometric forms in the viewer (line, 13 | // circles, points, etc.) 14 | *********************************************************************/ 15 | 16 | #define DEFAULT_IMAGE_WIDTH 320 17 | #define DEFAULT_IMAGE_HEIGHT 240 18 | #define DEFAULT_IMAGE_DEPTH 32 19 | 20 | #include "ImageViewer.h" 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | ////////////////////////////////////////////////////////////////////////// 34 | // constructor 35 | ////////////////////////////////////////////////////////////////////////// 36 | ImageViewer::ImageViewer() : imageBuffer_(NULL) 37 | { 38 | // setBackgroundMode(Qt::NoBackground); 39 | setBackgroundRole(QPalette::Window); 40 | imageBuffer_ = new QPixmap(DEFAULT_IMAGE_WIDTH, DEFAULT_IMAGE_HEIGHT); 41 | resize(imageBuffer_->size()); 42 | } 43 | 44 | 45 | ////////////////////////////////////////////////////////////////////////// 46 | // destructor 47 | 48 | ////////////////////////////////////////////////////////////////////////// 49 | ImageViewer::~ImageViewer() 50 | { 51 | delete imageBuffer_; 52 | } 53 | 54 | ////////////////////////////////////////////////////////////////////////// 55 | // slot 56 | // connect this to the signal that will send the image 57 | ////////////////////////////////////////////////////////////////////////// 58 | void ImageViewer::display(QImage * image) 59 | { 60 | //tic(); 61 | imageMutex_->lock(); 62 | if ( (image == NULL) || (image->isNull()) ) 63 | { 64 | qDebug() << "ImageViewer: image is no more valid, not displayed"; 65 | } 66 | else 67 | { 68 | if (imageBuffer_->size() != image->size()) 69 | resize(image->size()); 70 | *imageBuffer_ = QPixmap::fromImage(*image); 71 | update(); 72 | } 73 | imageMutex_->unlock(); 74 | //toc("imageViewer"); 75 | 76 | } 77 | 78 | 79 | 80 | ////////////////////////////////////////////////////////////////////////// 81 | // slot 82 | // activated when repaint() is called 83 | ////////////////////////////////////////////////////////////////////////// 84 | void ImageViewer::paintEvent(QPaintEvent * e) 85 | { 86 | QPainter painter(this); 87 | painter.setClipRect(e->rect()); 88 | if (imageBuffer_) { 89 | painter.drawPixmap(0,0,*imageBuffer_); 90 | } 91 | } 92 | 93 | ////////////////////////////////////////////////////////////////////////// 94 | void ImageViewer::setMutex(QMutex * imageMutex) 95 | { 96 | imageMutex_ = imageMutex; 97 | } 98 | 99 | void ImageViewer::tic() 100 | { 101 | tic_ = road_time(); 102 | } 103 | 104 | void ImageViewer::toc(char * text) 105 | { 106 | cout << "duration = " << (int)(road_time() - tic_) << " " << text << "\n"; 107 | } 108 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/ImageViewer.h.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2006/06/09 - 7:58 3 | // filename: ImageViewer.h 4 | // 5 | // author: Gerald Dherbomez 6 | // 7 | // version: $Id: ImageViewer.h 1239 2012-11-28 16:30:00Z kurdejma $ 8 | // 9 | // purpose: use this class to display an image 10 | *********************************************************************/ 11 | 12 | #ifndef IMAGEVIEWER_H 13 | #define IMAGEVIEWER_H 14 | 15 | #include "StdDbtPlayerComponentsConfig.h" 16 | #include "kernel/road_time.h" 17 | 18 | //# include "../../include/VisualMemoryManager/struct.h" 19 | 20 | #include 21 | 22 | class QMutex; 23 | 24 | class STDDBTPLAYERCOMPONENTS_API ImageViewer 25 | : public QFrame 26 | { 27 | Q_OBJECT 28 | 29 | public: 30 | ImageViewer(); 31 | ~ImageViewer(); 32 | 33 | void setMutex(QMutex * imageMutex); 34 | 35 | public Q_SLOTS: 36 | void display(QImage * image); 37 | 38 | protected Q_SLOTS: 39 | void paintEvent(QPaintEvent * e); 40 | 41 | private: 42 | QPixmap * imageBuffer_; // the pixmap that will be displayed 43 | QMutex * imageMutex_; // the mutex that protects access to the shared image 44 | 45 | road_time_t tic_; 46 | void tic(); 47 | void toc(char * text); 48 | }; 49 | 50 | #endif // IMAGEVIEWER_H 51 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/.svn/text-base/StdDbtPlayerComponentsConfig.h.svn-base: -------------------------------------------------------------------------------- 1 | #ifndef __STDDBTPLAYERCOMPONENTS_H__ 2 | #define __STDDBTPLAYERCOMPONENTS_H__ 3 | 4 | // Export macro for CanGateway DLL for Windows only 5 | #ifdef WIN32 6 | # ifdef STDDBTPLAYERCOMPONENTS_EXPORTS 7 | // make DLL 8 | # define STDDBTPLAYERCOMPONENTS_API __declspec(dllexport) 9 | # else 10 | // use DLL 11 | # define STDDBTPLAYERCOMPONENTS_API __declspec(dllimport) 12 | # endif 13 | #else 14 | // On other platforms, simply ignore this 15 | # define STDDBTPLAYERCOMPONENTS_API 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(StdDbtPlayerComponents) 2 | 3 | ################################################################################ 4 | add_definitions( -DSTDDBTPLAYERCOMPONENTS_EXPORTS ) 5 | 6 | # ======================================== 7 | # Configure qt4 8 | # ======================================== 9 | if(QT4_FOUND) 10 | set(QT_USE_QTXML true) 11 | set(QT_USE_QTNETWORK true) 12 | include(${QT_USE_FILE}) 13 | else() 14 | message(ERROR "Qt4 needed") 15 | endif() 16 | 17 | # ======================================== 18 | # Compiler definitions 19 | # ======================================== 20 | add_definitions( 21 | ${QT_DEFINITIONS} 22 | ) 23 | 24 | # ======================================== 25 | # Include directories 26 | # ======================================== 27 | include_directories( 28 | ${PROJECT_BINARY_DIR} 29 | ${QT_INCLUDE_DIR} 30 | ) 31 | 32 | # ======================================== 33 | # Link directories 34 | # ======================================== 35 | link_directories( ${PACPUS_LIB_DIR} 36 | ) 37 | 38 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME} ) 39 | 40 | set(HDRS 41 | DbtPlyImageManager.h 42 | DbtPlyStereoManager.h 43 | ImageViewer.h 44 | StdDbtPlayerComponentsConfig.h 45 | DbtPlyVelodyneManager.h 46 | ) 47 | 48 | 49 | # ======================================== 50 | # List of sources 51 | # ======================================== 52 | set( 53 | PROJECT_SRCS 54 | DbtPlyImageManager.cpp 55 | DbtPlyStereoManager.cpp 56 | DbtPlyVelodyneManager.cpp 57 | ImageViewer.cpp 58 | ${HDRS} 59 | ${PLUGIN_CPP} 60 | ) 61 | 62 | # ======================================== 63 | # Files to MOC 64 | # ======================================== 65 | set( 66 | FILES_TO_MOC 67 | DbtPlyImageManager.h 68 | DbtPlyStereoManager.h 69 | DbtPlyVelodyneManager.h 70 | ImageViewer.h 71 | ${PLUGIN_H} 72 | ) 73 | 74 | 75 | set( 76 | UI_FILES 77 | 78 | ) 79 | 80 | # ======================================== 81 | # Call MOC 82 | # ======================================== 83 | qt4_wrap_cpp( 84 | PROJECT_MOC_SRCS 85 | ${FILES_TO_MOC} 86 | ) 87 | 88 | qt4_wrap_ui( 89 | PROJECT_UI_SRCS 90 | ${UI_FILES} 91 | ) 92 | 93 | # ======================================== 94 | # Build a library 95 | # ======================================== 96 | pacpus_add_library( 97 | ${PROJECT_NAME} SHARED 98 | ${PROJECT_SRCS} 99 | ${PROJECT_MOC_SRCS} 100 | ${PROJECT_UI_SRCS} 101 | ) 102 | 103 | set(LIBS 104 | optimized FileLib debug FileLib_d 105 | optimized PacpusLib debug PacpusLib_d 106 | optimized PacpusTools debug PacpusTools_d 107 | ) 108 | if (WIN32) 109 | list(APPEND LIBS 110 | optimized ROAD_TIME debug ROAD_TIME_d 111 | ) 112 | endif() 113 | 114 | # ======================================== 115 | # Libraries 116 | # ======================================== 117 | # All the platform 118 | target_link_libraries( 119 | ${PROJECT_NAME} 120 | ${PACPUS_LIBRARIES} 121 | ${QT_LIBRARIES} 122 | ${PACPUS_DEPENDENCIES_LIB} 123 | ${LIBS} 124 | ) 125 | 126 | pacpus_folder(${PROJECT_NAME} "components") 127 | 128 | # ======================================== 129 | # Install 130 | # ======================================== 131 | pacpus_install(${PROJECT_NAME}) 132 | 133 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyImageManager.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2007/04/12 - 16:30 3 | // 4 | // author: Elie Al Alam & Gerald Dherbomez 5 | // 6 | // version: $Id: DbtPlyImageManager.cpp 1239 2012-11-28 16:30:00Z kurdejma $ 7 | // 8 | // purpose: Dbite Player Image Manager implementation 9 | *********************************************************************/ 10 | 11 | #include "DbtPlyImageManager.h" 12 | 13 | #include "ImageViewer.h" 14 | 15 | #include "kernel/ComponentFactory.h" 16 | #include "kernel/ComponentManager.h" 17 | #include "kernel/DbiteFileTypes.h" 18 | #include "kernel/Log.h" 19 | #include "PacpusTools/ShMem.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace pacpus { 26 | 27 | using namespace std; 28 | 29 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyImageManager"); 30 | 31 | static const string kDefaultMemoryName = "IMAGE"; 32 | 33 | /// Construction de la fabrique de composant DbtPlyImageManager 34 | static ComponentFactory sFactory("DbtPlyImageManager"); 35 | 36 | /// Constructor 37 | DbtPlyImageManager::DbtPlyImageManager(QString name) 38 | : DbtPlyFileManager (name) 39 | , im_(NULL) 40 | , shMem_(NULL) 41 | , imageMutex_(new QMutex()) 42 | , firstTime(true) 43 | { 44 | } 45 | 46 | /// Destructor 47 | DbtPlyImageManager::~DbtPlyImageManager() 48 | { 49 | // #if WIN32 50 | delete imageMutex_; 51 | // #endif 52 | if (shMem_) { 53 | delete shMem_; 54 | shMem_ = NULL; 55 | } 56 | } 57 | 58 | /// TODO: doc 59 | void DbtPlyImageManager::processData(road_time_t /*t*/, road_timerange_t /*tr*/, void * buf) 60 | { 61 | if (!buf) { 62 | LOG_WARN("no data to process: empty data buffer"); 63 | return; 64 | } 65 | 66 | // look at the dbt index in file manager and get the identifier of dbt structure 67 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType(); 68 | 69 | switch (id) { 70 | case STEREO_LEFT_IMAGE: 71 | case STEREO_RIGHT_IMAGE: 72 | //{ 73 | // QMutexLocker mutexLocker(imageMutex_); 74 | // Q_UNUSED(mutexLocker); 75 | // if (firstTime) { 76 | // im_ = new QImage(320, 240, QImage::Format_RGB32); 77 | // if (!im_) { 78 | // LOG_ERROR("invalid image"); 79 | // } 80 | // shMem_ = new ShMem(kDefaultMemoryName.c_str(), im_->numBytes()); 81 | // firstTime = false; 82 | // } 83 | // // convert image to 32 bits for the display 84 | // YtoRGB32(im_->bits(), (unsigned char *)buf); 85 | // shMem_->write(im_->bits(), im_->numBytes()); 86 | //} 87 | // break; 88 | 89 | case FILE_JPEG: 90 | imageFile_ = mDbtDataPath + (char *)buf; 91 | LOG_TRACE("image path = " << imageFile_); 92 | if (mVerbose) { 93 | cout << "[IMAGE]:\t" 94 | << imageFile_.toStdString() << endl 95 | ; 96 | } 97 | 98 | { 99 | QMutexLocker mutexLocker(imageMutex_); 100 | Q_UNUSED(mutexLocker); 101 | if (!im_) { 102 | LOG_TRACE("allocating image"); 103 | im_ = new QImage(); 104 | if (!im_) { 105 | LOG_ERROR("cannot allocate image"); 106 | } 107 | } 108 | static int shmemSize; 109 | bool imageLoaded = im_->load(imageFile_); 110 | if (!imageLoaded) { 111 | LOG_WARN("cannot load image file '" << imageFile_ << "'"); 112 | return; 113 | } 114 | LOG_TRACE("image loaded"); 115 | if (firstTime) { 116 | // send image in shared memory 117 | shmemSize = im_->byteCount(); 118 | LOG_TRACE("'" << mShMemName << "'" << " memory size = " << shmemSize); 119 | shMem_ = new ShMem(mShMemName.toStdString().c_str(), shmemSize); 120 | firstTime = false; 121 | } 122 | shMem_->write(im_->bits(), shmemSize); 123 | } 124 | break; 125 | 126 | default: 127 | break; 128 | } 129 | 130 | Q_EMIT displayIm(im_); 131 | } 132 | 133 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyImageManager::configureComponent(XmlComponentConfig config) 134 | { 135 | mShMemName = kDefaultMemoryName.c_str(); 136 | if (param.getProperty("shmem") != QString::null) { 137 | mShMemName = param.getProperty("shmem"); 138 | } 139 | return DbtPlyFileManager::configureComponent(config); 140 | } 141 | 142 | void DbtPlyImageManager::startActivity() 143 | { 144 | DbtPlyFileManager::startActivity(); 145 | } 146 | 147 | void DbtPlyImageManager::stopActivity() 148 | { 149 | delete im_; im_ = NULL; 150 | DbtPlyFileManager::stopActivity(); 151 | } 152 | 153 | //////////////////////////////////////////////////////////////////////////////// 154 | /// Pour l'affichage de l'image 155 | void DbtPlyImageManager::displayUI() 156 | { 157 | imviewer_ = new ImageViewer; 158 | imviewer_->setMutex(imageMutex_); 159 | imviewer_->show(); 160 | imviewer_->setWindowTitle(componentName); 161 | connect(this, SIGNAL(displayIm(QImage*)), imviewer_, SLOT(display(QImage*))); 162 | } 163 | 164 | //////////////////////////////////////////////////////////////////////////////// 165 | /// Convert the image from Y to RGB32 166 | /// The image is stored using a 32-bit RGB format (0xffRRGGBB) 167 | void DbtPlyImageManager::YtoRGB32(unsigned char * dest, unsigned char * src) 168 | { 169 | unsigned char *srcptr, *srcend, *destptr; 170 | 171 | srcptr = src; 172 | srcend = srcptr + (320 * 240); 173 | destptr = dest; 174 | 175 | // single-stage idiotproofing 176 | if (src == NULL || dest == NULL) 177 | return; 178 | 179 | // just Y's (monochrome) 180 | 181 | // unroll it to 4 per cycle 182 | 183 | while(srcptr < srcend) { 184 | *destptr++ = *srcptr; 185 | *destptr++ = *srcptr; 186 | *destptr++ = *srcptr; 187 | destptr++;//A 188 | srcptr++; 189 | 190 | *destptr++ = *srcptr; 191 | *destptr++ = *srcptr; 192 | *destptr++ = *srcptr; 193 | destptr++;//A 194 | srcptr++; 195 | 196 | *destptr++ = *srcptr; 197 | *destptr++ = *srcptr; 198 | *destptr++ = *srcptr; 199 | destptr++;//A 200 | srcptr++; 201 | 202 | *destptr++ = *srcptr; 203 | *destptr++ = *srcptr; 204 | *destptr++ = *srcptr; 205 | destptr++;//A 206 | srcptr++; 207 | } 208 | } 209 | 210 | void DbtPlyImageManager::tic() 211 | { 212 | tic_ = road_time(); 213 | } 214 | 215 | void DbtPlyImageManager::toc(char * text) 216 | { 217 | cout << "duration = " << (int)(road_time() - tic_) << "\t" 218 | << text << "\n" 219 | ; 220 | } 221 | 222 | } // namespace pacpus 223 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyImageManager.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2007/04/12 - 16:30 3 | 4 | // 5 | // author: Elie Al Alam & Gerald Dherbomez 6 | // 7 | // version: $Id: $ 8 | // 9 | // purpose: Dbite Player Image Manager header file 10 | *********************************************************************/ 11 | 12 | #ifndef DBTPLYIMAGEMANAGER_H 13 | #define DBTPLYIMAGEMANAGER_H 14 | 15 | #include 16 | 17 | #include "StdDbtPlayerComponentsConfig.h" 18 | #include "DbitePlayer/DbtPlyFileManager.h" 19 | 20 | class ImageViewer; 21 | 22 | class QImage; 23 | class QMutex; 24 | 25 | namespace pacpus { 26 | 27 | class ShMem; 28 | 29 | /// 30 | /// shmem=STRING (name of output shared memory) default=IMAGE 31 | class STDDBTPLAYERCOMPONENTS_API DbtPlyImageManager 32 | : public DbtPlyFileManager 33 | { 34 | Q_OBJECT 35 | 36 | public: 37 | DbtPlyImageManager(QString name); 38 | ~DbtPlyImageManager(); 39 | 40 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 41 | 42 | void displayUI(); 43 | 44 | private: 45 | QString mShMemName; 46 | QString imageFile_; 47 | QImage * im_; 48 | 49 | ImageViewer * imviewer_; 50 | 51 | protected: 52 | void processData(road_time_t, road_timerange_t, void * dataBuffer); 53 | virtual void startActivity(); 54 | virtual void stopActivity(); 55 | 56 | Q_SIGNALS: 57 | void displayIm(QImage *); 58 | 59 | private: 60 | QMutex * imageMutex_; 61 | ShMem * shMem_; 62 | bool firstTime; 63 | 64 | road_time_t tic_; 65 | void tic(); 66 | void toc(char * text); 67 | 68 | void YtoRGB32(unsigned char * dest, unsigned char * src); 69 | }; 70 | 71 | } // namespace pacpus 72 | 73 | #endif // DBTPLYIMAGEMANAGER_H 74 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyStereoManager.cpp: -------------------------------------------------------------------------------- 1 | /// purpose: Dbite Player Stereo Manager implementation 2 | /// 3 | /// created @date 2008/01/19 - 21:23 4 | /// @author Sergio Rodriguez 5 | /// @version $Id: $ 6 | 7 | #include "DbtPlyStereoManager.h" 8 | 9 | #include 10 | 11 | #include "kernel/ComponentManager.h" 12 | #include "kernel/DbiteFileTypes.h" 13 | #include "kernel/Log.h" 14 | 15 | namespace pacpus { 16 | 17 | using namespace std; 18 | 19 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyStereoManager"); 20 | 21 | /// Construction de la fabrique de composant DbtPlyStereoManager 22 | static ComponentFactory sFactory("DbtPlyStereoManager"); 23 | 24 | DbtPlyStereoManager::DbtPlyStereoManager(QString name) 25 | : DbtPlyFileManager(name) 26 | { 27 | firstTime = true; 28 | tobeRead = NULL; 29 | 30 | left0 = NULL; 31 | right0 = NULL; 32 | timeStamp0 = NULL; 33 | timeRange0 = NULL; 34 | 35 | left1 = NULL; 36 | right1 = NULL; 37 | timeStamp1 = NULL; 38 | timeRange1 = NULL; 39 | } 40 | 41 | DbtPlyStereoManager::~DbtPlyStereoManager() 42 | { 43 | delete tobeRead; 44 | delete left0; 45 | delete right0; 46 | delete timeStamp0; 47 | delete timeRange0; 48 | delete left1; 49 | delete right1; 50 | delete timeStamp1; 51 | delete timeRange1; 52 | } 53 | 54 | //processData unix 55 | void DbtPlyStereoManager::processData(road_time_t t, road_timerange_t tr, void * buf) 56 | { 57 | if (!buf) { 58 | return; 59 | } 60 | 61 | const size_t imageSize = width_ * height_ * depth_; 62 | 63 | // look at the dbt index in file manager and get the identifier of dbt structure 64 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType(); 65 | 66 | if (firstTime) { 67 | tobeRead = new ShMem("OFFSET", sizeof(bool)); //Offset pointer to read 68 | 69 | left0 = new ShMem("BufferLeft0", imageSize); 70 | right0 = new ShMem("BufferRight0", imageSize); 71 | timeStamp0 = new ShMem("BufferTimeStamp0", sizeof(road_time_t)); 72 | timeRange0 = new ShMem("BufferTimeRange0", sizeof(road_timerange_t)); 73 | 74 | left1 = new ShMem("BufferLeft1", imageSize); 75 | right1 = new ShMem("BufferRight1", imageSize); 76 | timeStamp1 = new ShMem("BufferTimeStamp1", sizeof(road_time_t)); 77 | timeRange1 = new ShMem("BufferTimeRange1", sizeof(road_timerange_t)); 78 | 79 | firstTime = false; 80 | leftStatus = false; 81 | rightStatus = false; 82 | offset = false; 83 | 84 | if (tobeRead == NULL) { 85 | LOG_FATAL("Crashing because of shared memory"); 86 | } 87 | } 88 | 89 | //dropping stereo images 90 | switch (id) { 91 | case STEREO_LEFT_IMAGE: 92 | if (!offset) 93 | left0->write(buf, imageSize); 94 | else 95 | left1->write(buf, imageSize); 96 | leftStatus = true; 97 | break; 98 | case STEREO_RIGHT_IMAGE: 99 | if (!offset) 100 | right0->write(buf, imageSize); 101 | else 102 | right1->write(buf, imageSize); 103 | rightStatus = true; 104 | break; 105 | default: 106 | //Unkown id data type 107 | break; 108 | } 109 | 110 | if ((leftStatus)&&(rightStatus)) { // synchronized stereo data is available 111 | //dropping time stamp 112 | if (!offset) { 113 | timeStamp0->write(&t, sizeof(road_time_t)); 114 | timeRange0->write(&tr, sizeof(road_timerange_t)); 115 | } else { 116 | timeStamp1->write(&t, sizeof(road_time_t)); 117 | timeRange1->write(&tr, sizeof(road_timerange_t)); 118 | } 119 | if (mVerbose >= 1) { 120 | cout << "[Shmem update Event] at " << t << std::endl; 121 | } 122 | //setting read flag 123 | tobeRead->write((char*)&offset, sizeof(bool)); 124 | if (!offset) 125 | offset = true; 126 | else 127 | offset =false; 128 | 129 | leftStatus = false; 130 | rightStatus = false; 131 | } 132 | } 133 | 134 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyStereoManager::configureComponent(XmlComponentConfig config) 135 | { 136 | //default stereo image parameters 137 | width_ = 320; // pixels 138 | height_ = 240; // pixels 139 | depth_ = 1; // bytes-per-pixel 140 | 141 | //load stereo image parameters 142 | width_ = param.getProperty("width").toInt(); 143 | height_ = param.getProperty("height").toInt(); 144 | depth_ = param.getProperty("depth").toInt(); 145 | 146 | return DbtPlyFileManager::configureComponent(config); 147 | } 148 | 149 | void DbtPlyStereoManager::startActivity() 150 | { 151 | DbtPlyFileManager::startActivity(); 152 | } 153 | 154 | void DbtPlyStereoManager::stopActivity() 155 | { 156 | DbtPlyFileManager::stopActivity(); 157 | } 158 | 159 | void DbtPlyStereoManager::displayUI() 160 | { 161 | // TODO: not implemented 162 | } 163 | 164 | void DbtPlyStereoManager::tic() 165 | { 166 | tic_ = road_time(); 167 | } 168 | 169 | void DbtPlyStereoManager::toc(char * text) 170 | { 171 | cout << "duration = " << (int)(road_time() - tic_) << " " << text << "\n"; 172 | } 173 | 174 | } // namespace pacpus 175 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyStereoManager.h: -------------------------------------------------------------------------------- 1 | /// purpose: Dbite Player Stereo Manager header file 2 | /// 3 | /// created @date 2008/01/19 - 21:23 4 | /// @author Sergio Rodriguez 5 | /// @version $Id: $ 6 | 7 | #ifndef DBTPLYSTEREOMANAGER_H 8 | #define DBTPLYSTEREOMANAGER_H 9 | 10 | #include "StdDbtPlayerComponentsConfig.h" 11 | #include "kernel/ComponentBase.h" 12 | #include "kernel/ComponentFactory.h" 13 | #include "DbitePlayer/DbtPlyFileManager.h" 14 | #include "PacpusTools/ShMem.h" 15 | 16 | namespace pacpus { 17 | 18 | class STDDBTPLAYERCOMPONENTS_API DbtPlyStereoManager 19 | : public DbtPlyFileManager 20 | { 21 | Q_OBJECT 22 | 23 | public: 24 | DbtPlyStereoManager(QString name); 25 | ~DbtPlyStereoManager(); 26 | 27 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 28 | void displayUI(); 29 | 30 | protected: 31 | void processData(road_time_t t, road_timerange_t tr, void * buffer); 32 | virtual void startActivity(); 33 | virtual void stopActivity(); 34 | 35 | private: 36 | ShMem * tobeRead; //Buffer flag 37 | 38 | ShMem * left0; //left image of Buffer 0 39 | ShMem * right0; //right image of Buffer 0 40 | ShMem * timeStamp0; //Timestamp of stereo images in Buffer 0 41 | ShMem * timeRange0; //Uncertainty of timeStamp Buffer 0 42 | 43 | ShMem * left1; //left image of Buffer 1 44 | ShMem * right1; //right image of Buffer 1 45 | ShMem * timeStamp1; //Timestamp of stereo images in Buffer 1 46 | ShMem * timeRange1; //Uncertainty of timeStamp Buffer 1 47 | //#endif 48 | 49 | bool firstTime; 50 | bool offset; 51 | bool leftStatus; 52 | bool rightStatus; 53 | //image paramaters 54 | int width_; 55 | int height_; 56 | int depth_; 57 | 58 | road_time_t tic_; 59 | void tic(); 60 | void toc(char * text); 61 | }; 62 | 63 | } // namespace pacpus 64 | 65 | #endif // DBTPLYSTEREOMANAGER_H 66 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyVelodyneManager.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/04/15 3 | // 4 | // author: Gerald Dherbomez 5 | // 6 | // version: $Id: $ 7 | // 8 | // purpose: Dbite Player Velodyne Manager implementation 9 | *********************************************************************/ 10 | 11 | #include "DbtPlyVelodyneManager.h" 12 | 13 | #include "kernel/ComponentFactory.h" 14 | #include "kernel/ComponentManager.h" 15 | #include "kernel/Log.h" 16 | 17 | #include 18 | #include 19 | 20 | namespace pacpus { 21 | 22 | using namespace std; 23 | 24 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyVelodyneManager"); 25 | 26 | static ComponentFactory sFactory("DbtPlyVelodyneManager"); 27 | 28 | static const string kVelodyneMemoryName = "VELODYNE"; 29 | 30 | /// Constructor. 31 | DbtPlyVelodyneManager::DbtPlyVelodyneManager(QString name) 32 | : DbtPlyFileManager (name) 33 | , shMem_(NULL) 34 | { 35 | LOG_TRACE("constructor"); 36 | } 37 | 38 | /// Destructor. 39 | DbtPlyVelodyneManager::~DbtPlyVelodyneManager() 40 | { 41 | LOG_TRACE("destructor"); 42 | } 43 | 44 | void DbtPlyVelodyneManager::processData(road_time_t /*time*/, road_timerange_t /*timeRange*/, void * buffer) 45 | { 46 | if (NULL == buffer) { 47 | LOG_WARN("processing data: NULL buffer"); 48 | return; 49 | } 50 | 51 | if (mVerbose) { 52 | VelodynePolarData & vpd = *(VelodynePolarData *)buffer; 53 | cout << "[VELODYNE]:\t" 54 | << "#range=" << vpd.range << "\n" 55 | ; 56 | } 57 | 58 | memcpy(&sphericData_, buffer, sizeof(VelodynePolarData) ); 59 | shMem_->write(&sphericData_, sizeof(VelodynePolarData) ); 60 | } 61 | 62 | void DbtPlyVelodyneManager::startActivity() 63 | { 64 | LOG_TRACE("starting activity..."); 65 | 66 | shMem_ = new ShMem(kVelodyneMemoryName.c_str(), sizeof(VelodynePolarData)); 67 | if (!shMem_) { 68 | LOG_FATAL("cannot create Velodyne shared memory"); 69 | } 70 | DbtPlyFileManager::startActivity(); 71 | LOG_TRACE("started activity"); 72 | } 73 | 74 | void DbtPlyVelodyneManager::stopActivity() 75 | { 76 | LOG_TRACE("stopping activity..."); 77 | 78 | DbtPlyFileManager::stopActivity(); 79 | delete shMem_; // it is ok to delete a NULL pointer 80 | shMem_ = NULL; 81 | LOG_TRACE("stopped activity"); 82 | } 83 | 84 | void DbtPlyVelodyneManager::displayUI() 85 | { 86 | } 87 | 88 | void DbtPlyVelodyneManager::tic() 89 | { 90 | tic_ = road_time(); 91 | } 92 | 93 | void DbtPlyVelodyneManager::toc(const char * text) 94 | { 95 | LOG_INFO("duration = " << (int)(road_time() - tic_) << " : " << text); 96 | } 97 | 98 | } // namespace pacpus 99 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyVelodyneManager.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/04/15 3 | // 4 | // author: Gerald Dherbomez 5 | // 6 | // version: $Id: $ 7 | // 8 | // purpose: Dbite Player Velodyne Manager definition 9 | *********************************************************************/ 10 | 11 | #ifndef DBTPLYVELODYNEMANAGER_H 12 | #define DBTPLYVELODYNEMANAGER_H 13 | 14 | #include 15 | 16 | #include "DbitePlayer/DbtPlyFileManager.h" 17 | #include "PacpusTools/ShMem.h" 18 | #include "../VelodyneComponent/structure_velodyne.h" 19 | 20 | // Export macro for DbtPlyVelodyne DLL for Windows only 21 | #ifdef WIN32 22 | # ifdef DBTPLYVELODYNE_EXPORTS 23 | // make DLL 24 | # define DBTPLYVELODYNE_API __declspec(dllexport) 25 | # else 26 | // use DLL 27 | # define DBTPLYVELODYNE_API __declspec(dllimport) 28 | # endif 29 | #else 30 | // On other platforms, simply ignore this 31 | # define DBTPLYVELODYNE_API 32 | #endif 33 | 34 | namespace pacpus { 35 | 36 | class DBTPLYVELODYNE_API DbtPlyVelodyneManager 37 | : public DbtPlyFileManager 38 | { 39 | Q_OBJECT 40 | 41 | public: 42 | DbtPlyVelodyneManager(QString name); 43 | virtual ~DbtPlyVelodyneManager(); 44 | 45 | void displayUI(); 46 | 47 | protected: 48 | void processData(road_time_t, road_timerange_t, void * dataBuffer); 49 | virtual void startActivity(); 50 | virtual void stopActivity(); 51 | 52 | private: 53 | ShMem * shMem_; 54 | 55 | VelodynePolarData sphericData_; 56 | 57 | road_time_t tic_; 58 | void tic(); 59 | void toc(const char * text); 60 | }; 61 | 62 | } // namespace pacpus 63 | 64 | #endif // DBTPLYVELODYNEMANAGER_H 65 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/DbtPlyVisionConfig.h: -------------------------------------------------------------------------------- 1 | #ifndef DBTPLYVISIONCONFIG_H 2 | #define DBTPLYVISIONCONFIG_H 3 | 4 | // Export macro for DbtPlyVision DLL for Windows only 5 | #ifdef WIN32 6 | # if defined(DBTPLYVISION_EXPORTS) || defined(STEREO_EXPORTS) 7 | // make DLL 8 | # define DBTPLYVISION_API __declspec(dllexport) 9 | # else 10 | // use DLL 11 | # define DBTPLYVISION_API __declspec(dllimport) 12 | # endif 13 | #else 14 | // On other platforms, simply ignore this 15 | # define DBTPLYVISION_API 16 | #endif 17 | 18 | #endif // DBTPLYVISIONCONFIG_H 19 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/ImageViewer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2006/06/09 - 9:40 3 | // filename: ImageViewer.cpp 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id$ 9 | // 10 | // purpose: Definition of the ImageViewer class 11 | // 12 | // todo : - think how to display geometric forms in the viewer (line, 13 | // circles, points, etc.) 14 | *********************************************************************/ 15 | 16 | #define DEFAULT_IMAGE_WIDTH 320 17 | #define DEFAULT_IMAGE_HEIGHT 240 18 | #define DEFAULT_IMAGE_DEPTH 32 19 | 20 | #include "ImageViewer.h" 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | ////////////////////////////////////////////////////////////////////////// 34 | // constructor 35 | ////////////////////////////////////////////////////////////////////////// 36 | ImageViewer::ImageViewer() : imageBuffer_(NULL) 37 | { 38 | // setBackgroundMode(Qt::NoBackground); 39 | setBackgroundRole(QPalette::Window); 40 | imageBuffer_ = new QPixmap(DEFAULT_IMAGE_WIDTH, DEFAULT_IMAGE_HEIGHT); 41 | resize(imageBuffer_->size()); 42 | } 43 | 44 | 45 | ////////////////////////////////////////////////////////////////////////// 46 | // destructor 47 | 48 | ////////////////////////////////////////////////////////////////////////// 49 | ImageViewer::~ImageViewer() 50 | { 51 | delete imageBuffer_; 52 | } 53 | 54 | ////////////////////////////////////////////////////////////////////////// 55 | // slot 56 | // connect this to the signal that will send the image 57 | ////////////////////////////////////////////////////////////////////////// 58 | void ImageViewer::display(QImage * image) 59 | { 60 | //tic(); 61 | imageMutex_->lock(); 62 | if ( (image == NULL) || (image->isNull()) ) 63 | { 64 | qDebug() << "ImageViewer: image is no more valid, not displayed"; 65 | } 66 | else 67 | { 68 | if (imageBuffer_->size() != image->size()) 69 | resize(image->size()); 70 | *imageBuffer_ = QPixmap::fromImage(*image); 71 | update(); 72 | } 73 | imageMutex_->unlock(); 74 | //toc("imageViewer"); 75 | 76 | } 77 | 78 | 79 | 80 | ////////////////////////////////////////////////////////////////////////// 81 | // slot 82 | // activated when repaint() is called 83 | ////////////////////////////////////////////////////////////////////////// 84 | void ImageViewer::paintEvent(QPaintEvent * e) 85 | { 86 | QPainter painter(this); 87 | painter.setClipRect(e->rect()); 88 | if (imageBuffer_) { 89 | painter.drawPixmap(0,0,*imageBuffer_); 90 | } 91 | } 92 | 93 | ////////////////////////////////////////////////////////////////////////// 94 | void ImageViewer::setMutex(QMutex * imageMutex) 95 | { 96 | imageMutex_ = imageMutex; 97 | } 98 | 99 | void ImageViewer::tic() 100 | { 101 | tic_ = road_time(); 102 | } 103 | 104 | void ImageViewer::toc(char * text) 105 | { 106 | cout << "duration = " << (int)(road_time() - tic_) << " " << text << "\n"; 107 | } 108 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/ImageViewer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2006/06/09 - 7:58 3 | // filename: ImageViewer.h 4 | // 5 | // author: Gerald Dherbomez 6 | // 7 | // version: $Id: ImageViewer.h 1239 2012-11-28 16:30:00Z kurdejma $ 8 | // 9 | // purpose: use this class to display an image 10 | *********************************************************************/ 11 | 12 | #ifndef IMAGEVIEWER_H 13 | #define IMAGEVIEWER_H 14 | 15 | #include "StdDbtPlayerComponentsConfig.h" 16 | #include "kernel/road_time.h" 17 | 18 | //# include "../../include/VisualMemoryManager/struct.h" 19 | 20 | #include 21 | 22 | class QMutex; 23 | 24 | class STDDBTPLAYERCOMPONENTS_API ImageViewer 25 | : public QFrame 26 | { 27 | Q_OBJECT 28 | 29 | public: 30 | ImageViewer(); 31 | ~ImageViewer(); 32 | 33 | void setMutex(QMutex * imageMutex); 34 | 35 | public Q_SLOTS: 36 | void display(QImage * image); 37 | 38 | protected Q_SLOTS: 39 | void paintEvent(QPaintEvent * e); 40 | 41 | private: 42 | QPixmap * imageBuffer_; // the pixmap that will be displayed 43 | QMutex * imageMutex_; // the mutex that protects access to the shared image 44 | 45 | road_time_t tic_; 46 | void tic(); 47 | void toc(char * text); 48 | }; 49 | 50 | #endif // IMAGEVIEWER_H 51 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/StdDbtPlayerComponentsConfig.h: -------------------------------------------------------------------------------- 1 | #ifndef __STDDBTPLAYERCOMPONENTS_H__ 2 | #define __STDDBTPLAYERCOMPONENTS_H__ 3 | 4 | // Export macro for CanGateway DLL for Windows only 5 | #ifdef WIN32 6 | # ifdef STDDBTPLAYERCOMPONENTS_EXPORTS 7 | // make DLL 8 | # define STDDBTPLAYERCOMPONENTS_API __declspec(dllexport) 9 | # else 10 | // use DLL 11 | # define STDDBTPLAYERCOMPONENTS_API __declspec(dllimport) 12 | # endif 13 | #else 14 | // On other platforms, simply ignore this 15 | # define STDDBTPLAYERCOMPONENTS_API 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/StdDbtPlayerComponentsPlugin.cpp: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #include "StdDbtPlayerComponentsPlugin.h" 5 | 6 | StdDbtPlayerComponentsPlugin::StdDbtPlayerComponentsPlugin() 7 | { 8 | } 9 | 10 | StdDbtPlayerComponentsPlugin::~StdDbtPlayerComponentsPlugin() 11 | { 12 | } 13 | 14 | QString StdDbtPlayerComponentsPlugin::name() 15 | { 16 | return "StdDbtPlayerComponentsPlugin"; 17 | } 18 | 19 | Q_EXPORT_PLUGIN2(StdDbtPlayerComponentsPlugin, StdDbtPlayerComponentsPlugin) 20 | -------------------------------------------------------------------------------- /pacpussensors/StdDbtPlayerComponents/StdDbtPlayerComponentsPlugin.h: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #ifndef __STDDBTPLAYERCOMPONENTSPLUGIN_H__ 5 | #define __STDDBTPLAYERCOMPONENTSPLUGIN_H__ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /// Auto-generated plugin class 13 | class StdDbtPlayerComponentsPlugin 14 | : public QObject 15 | , public PacpusPluginInterface 16 | { 17 | Q_OBJECT 18 | Q_INTERFACES(PacpusPluginInterface) 19 | 20 | public: 21 | StdDbtPlayerComponentsPlugin(); 22 | ~StdDbtPlayerComponentsPlugin(); 23 | 24 | protected: 25 | QString name(); 26 | }; 27 | 28 | #endif // __STDDBTPLAYERCOMPONENTSPLUGIN_H__ 29 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/--CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(VelodyneHDL64S2) 2 | 3 | ################################################################################ 4 | add_definitions( -DVELODYNEHDL64S2_EXPORTS ) 5 | 6 | ################################################################################ 7 | # FILES 8 | set(PROJECT_SOURCE_FILES 9 | # add your sources file to compile here 10 | VelodyneComponent.cpp 11 | ) 12 | 13 | set(FILES_TO_MOC 14 | # add here the header files that you want to MOC 15 | VelodyneComponent.h 16 | ) 17 | 18 | list(APPEND LIBS 19 | # add your specific libraries to link here 20 | ) 21 | 22 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME}) 23 | 24 | include_directories(${PACPUSINCLUDE_DIRECTORY}) 25 | link_directories(${PACPUSLIB_DIRECTORY}) 26 | link_directories(${PACPUSBIN_DIRECTORY}) 27 | 28 | message ("Creating source files for the plugin(s)") 29 | message(STATUS PLUGIN_H=${PLUGIN_H}) 30 | message(STATUS PLUGIN_CPP=${PLUGIN_CPP}) 31 | 32 | qt4_wrap_cpp(PLUGIN_MOC_SOURCES 33 | ${PLUGIN_H} 34 | ${FILES_TO_MOC} 35 | ) 36 | ################################################################################ 37 | # BUILD AND LINK 38 | add_library(${PROJECT_NAME} SHARED 39 | ${PROJECT_SOURCE_FILES} 40 | ${PLUGIN_CPP} 41 | ${PLUGIN_MOC_SOURCES} 42 | ) 43 | # LIBS 44 | list(APPEND LIBS 45 | optimized FileLib debug FileLib_d 46 | optimized PacpusLib debug PacpusLib_d 47 | optimized PacpusTools debug PacpusTools_d 48 | ${QT_LIBRARIES} 49 | ) 50 | if(WIN32) 51 | list(APPEND LIBS 52 | optimized ROAD_TIME debug ROAD_TIME_d 53 | ) 54 | endif() 55 | # LINK 56 | target_link_libraries(${PROJECT_NAME} ${LIBS}) 57 | # PROPERTIES 58 | set_target_properties(${PROJECT_NAME} PROPERTIES DEBUG_POSTFIX "_d") 59 | # FOLDERS 60 | if(PACPUS_FOLDERS) 61 | set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "components") 62 | endif() 63 | 64 | ################################################################################ 65 | # INSTALL 66 | install(TARGETS ${PROJECT_NAME} 67 | RUNTIME DESTINATION ${PACPUSBIN_DIRECTORY} 68 | LIBRARY DESTINATION ${PACPUSBIN_DIRECTORY} 69 | ARCHIVE DESTINATION ${PACPUSLIB_DIRECTORY} 70 | ) 71 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 53 4 | /svn/pacpusbase/!svn/ver/1272/trunk/VelodyneComponent 5 | END 6 | VelodyneComponent.cpp 7 | K 25 8 | svn:wc:ra_dav:version-url 9 | V 75 10 | /svn/pacpusbase/!svn/ver/1272/trunk/VelodyneComponent/VelodyneComponent.cpp 11 | END 12 | CMakeLists.txt 13 | K 25 14 | svn:wc:ra_dav:version-url 15 | V 68 16 | /svn/pacpusbase/!svn/ver/1188/trunk/VelodyneComponent/CMakeLists.txt 17 | END 18 | VelodyneViewer.h 19 | K 25 20 | svn:wc:ra_dav:version-url 21 | V 69 22 | /svn/pacpusbase/!svn/ver/861/trunk/VelodyneComponent/VelodyneViewer.h 23 | END 24 | VelodyneComponent.h 25 | K 25 26 | svn:wc:ra_dav:version-url 27 | V 73 28 | /svn/pacpusbase/!svn/ver/1199/trunk/VelodyneComponent/VelodyneComponent.h 29 | END 30 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/dir-prop-base: -------------------------------------------------------------------------------- 1 | K 10 2 | svn:ignore 3 | V 22 4 | *Plugin.cpp 5 | *Plugin.h 6 | 7 | END 8 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 1321 5 | https://devel.hds.utc.fr/svn/pacpusbase/trunk/VelodyneComponent 6 | https://devel.hds.utc.fr/svn/pacpusbase 7 | 8 | 9 | 10 | 2012-12-19T15:53:58.851666Z 11 | 1272 12 | kurdejma 13 | has-props 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 81eceb47-55a1-493d-9d98-39bc04a94090 28 | 29 | VelodyneComponent.cpp 30 | file 31 | 32 | 33 | 34 | 35 | 2013-10-22T12:44:53.748760Z 36 | 53a227f556269cac394d6e5adfd88f38 37 | 2012-12-19T15:53:58.851666Z 38 | 1272 39 | kurdejma 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 14232 62 | 63 | CMakeLists.txt 64 | file 65 | 66 | 67 | 68 | 69 | 2013-10-22T12:44:53.748760Z 70 | 9a763fa04131ca0c8e72c7cfeab39e43 71 | 2012-07-06T13:24:02.932929Z 72 | 1188 73 | kurdejma 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 1877 96 | 97 | VelodyneViewer.h 98 | file 99 | 100 | 101 | 102 | 103 | 2013-10-22T12:44:53.748760Z 104 | 3c69f6ec95476710758071a6fdfdea8a 105 | 2010-04-28T12:36:41.787126Z 106 | 861 107 | gdherbom 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 1812 130 | 131 | VelodyneComponent.h 132 | file 133 | 134 | 135 | 136 | 137 | 2013-10-22T12:44:53.748760Z 138 | 466774ae4e87d5519127bee40f572cf1 139 | 2012-08-01T08:51:24.322568Z 140 | 1199 141 | kurdejma 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 3107 164 | 165 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/text-base/CMakeLists.txt.svn-base: -------------------------------------------------------------------------------- 1 | project(VelodyneHDL64S2) 2 | 3 | ################################################################################ 4 | add_definitions( -DVELODYNEHDL64S2_EXPORTS ) 5 | 6 | ################################################################################ 7 | # FILES 8 | set(PROJECT_SOURCE_FILES 9 | # add your sources file to compile here 10 | VelodyneComponent.cpp 11 | ) 12 | 13 | set(FILES_TO_MOC 14 | # add here the header files that you want to MOC 15 | VelodyneComponent.h 16 | ) 17 | 18 | list(APPEND LIBS 19 | # add your specific libraries to link here 20 | ) 21 | 22 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME}) 23 | 24 | include_directories(${PACPUSINCLUDE_DIRECTORY}) 25 | link_directories(${PACPUSLIB_DIRECTORY}) 26 | link_directories(${PACPUSBIN_DIRECTORY}) 27 | 28 | message ("Creating source files for the plugin(s)") 29 | message(STATUS PLUGIN_H=${PLUGIN_H}) 30 | message(STATUS PLUGIN_CPP=${PLUGIN_CPP}) 31 | 32 | qt4_wrap_cpp(PLUGIN_MOC_SOURCES 33 | ${PLUGIN_H} 34 | ${FILES_TO_MOC} 35 | ) 36 | ################################################################################ 37 | # BUILD AND LINK 38 | add_library(${PROJECT_NAME} SHARED 39 | ${PROJECT_SOURCE_FILES} 40 | ${PLUGIN_CPP} 41 | ${PLUGIN_MOC_SOURCES} 42 | ) 43 | # LIBS 44 | list(APPEND LIBS 45 | optimized FileLib debug FileLib_d 46 | optimized PacpusLib debug PacpusLib_d 47 | optimized PacpusTools debug PacpusTools_d 48 | ${QT_LIBRARIES} 49 | ) 50 | if(WIN32) 51 | list(APPEND LIBS 52 | optimized ROAD_TIME debug ROAD_TIME_d 53 | ) 54 | endif() 55 | # LINK 56 | target_link_libraries(${PROJECT_NAME} ${LIBS}) 57 | # PROPERTIES 58 | set_target_properties(${PROJECT_NAME} PROPERTIES DEBUG_POSTFIX "_d") 59 | # FOLDERS 60 | if(PACPUS_FOLDERS) 61 | set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "components") 62 | endif() 63 | 64 | ################################################################################ 65 | # INSTALL 66 | install(TARGETS ${PROJECT_NAME} 67 | RUNTIME DESTINATION ${PACPUSBIN_DIRECTORY} 68 | LIBRARY DESTINATION ${PACPUSBIN_DIRECTORY} 69 | ARCHIVE DESTINATION ${PACPUSLIB_DIRECTORY} 70 | ) 71 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/text-base/VelodyneComponent.cpp.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/03/26 - 14:38 3 | // filename: VelodyneComponent.cpp 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id: $ 9 | // 10 | // purpose: The acquisition component of the Velodyne HDL64S2 sensor 11 | // 12 | *********************************************************************/ 13 | 14 | #include "VelodyneComponent.h" 15 | 16 | #include "kernel/cstdint.h" 17 | #include "kernel/ComponentFactory.h" 18 | #include "kernel/DbiteFileTypes.h" 19 | #include "kernel/Log.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace pacpus; 26 | using namespace std; 27 | 28 | DECLARE_STATIC_LOGGER("pacpus.base.VelodyneComponent"); 29 | 30 | /// Construct the factory 31 | static ComponentFactory sFactory("VelodyneComponent"); 32 | 33 | /// Default host address = 0.0.0.0 (any address) 34 | static const string kDefaultHostAddress = "0.0.0.0"; 35 | 36 | /// Default host port = 2368 37 | static const uint16_t kDefaultHostPort = 2368; 38 | 39 | static const string kPropertyRecording = "recording"; 40 | 41 | static const string kVelodyneSharedMemoryName = "VELODYNE"; 42 | static const string kDefaultOutputFilename = "velodyne_spheric.dbt"; 43 | 44 | ////////////////////////////////////////////////////////////////////////// 45 | /// Constructor 46 | VelodyneComponent::VelodyneComponent(QString name) 47 | : ComponentBase(name) 48 | , mPort(kDefaultHostPort) 49 | { 50 | LOG_TRACE("constructor(" << name << ")"); 51 | 52 | ComponentManager * mgr = ComponentManager::getInstance(); 53 | LOG_DEBUG("manager = " << mgr); 54 | 55 | recording = true; 56 | 57 | // default values 58 | if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) { 59 | LOG_ERROR("cannot set address"); 60 | } 61 | LOG_DEBUG("host address = " << mHost.toString()); 62 | } 63 | 64 | ////////////////////////////////////////////////////////////////////////// 65 | /// Destructor 66 | VelodyneComponent::~VelodyneComponent() 67 | { 68 | } 69 | 70 | ////////////////////////////////////////////////////////////////////////// 71 | /// Called by the ComponentManager to start the component 72 | void VelodyneComponent::startActivity() 73 | { 74 | mStartOfScan = false; 75 | mEndOfScan = false; 76 | mBlockIndex = 0; 77 | mCurrentVelodyneData = 0; 78 | mPreviousAngle = 0; 79 | mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]); 80 | 81 | mRunning = true; 82 | initialize(); 83 | } 84 | 85 | ////////////////////////////////////////////////////////////////////////// 86 | /// TODO: doc 87 | void VelodyneComponent::run() 88 | { 89 | initialize(); 90 | exec(); // launch the exec loop, blocking until receiving exit() signal ... 91 | close(); 92 | } 93 | 94 | ////////////////////////////////////////////////////////////////////////// 95 | /// TODO: doc 96 | void VelodyneComponent::initialize() 97 | { 98 | if (recording) { 99 | mVelodyneSphericDataFile.open(kDefaultOutputFilename, WriteMode, VELODYNE_RAW_SPHERIC_DATA, sizeof(VelodynePolarData)); 100 | } 101 | 102 | initSocket(); 103 | if (!connect(mSocket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()))) { 104 | LOG_ERROR("cannot connect SIGNAL(readyRead()) to SLOT(readPendingDatagrams())"); 105 | } 106 | 107 | mShMem = new ShMem(kVelodyneSharedMemoryName.c_str(), sizeof(VelodynePolarData) ); 108 | if (!mShMem) { 109 | LOG_FATAL("cannot create Velodyne shared memory"); 110 | return; 111 | } 112 | } 113 | 114 | ////////////////////////////////////////////////////////////////////////// 115 | /// TODO: doc 116 | void VelodyneComponent::initSocket() 117 | { 118 | if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) { 119 | LOG_FATAL("failed to set address"); 120 | } 121 | mPort = kDefaultHostPort; 122 | mSocket = new QUdpSocket(); 123 | if (!mSocket) { 124 | LOG_FATAL("cannot create socket"); 125 | return; 126 | } 127 | if (!(mSocket->bind(mHost, mPort))) { 128 | LOG_ERROR("error when binding velodyne"); 129 | } 130 | } 131 | 132 | ////////////////////////////////////////////////////////////////////////// 133 | /// Called by the ComponentManager to stop the component 134 | void VelodyneComponent::stopActivity() 135 | { 136 | mRunning = false; 137 | /* 138 | exit(); 139 | if (!wait(2000)) { 140 | // wait termination during 2 seconds 141 | LOG_ERROR("thread blocking, terminate it"); 142 | terminate(); 143 | } 144 | */ 145 | 146 | close(); 147 | } 148 | 149 | ////////////////////////////////////////////////////////////////////////// 150 | /// TODO: doc 151 | void VelodyneComponent::close() 152 | { 153 | closeSocket(); 154 | 155 | if (recording) { 156 | mVelodyneSphericDataFile.close(); 157 | } 158 | 159 | if (mShMem) { 160 | delete mShMem; 161 | mShMem = NULL; 162 | } 163 | } 164 | 165 | ////////////////////////////////////////////////////////////////////////// 166 | /// TODO: doc 167 | void VelodyneComponent::closeSocket() 168 | { 169 | if (mSocket) { 170 | mSocket->close(); 171 | delete mSocket; 172 | mSocket = NULL; 173 | } 174 | } 175 | 176 | ////////////////////////////////////////////////////////////////////////// 177 | /// Called by the ComponentManager to pass the XML parameters to the 178 | /// component 179 | ComponentBase::COMPONENT_CONFIGURATION VelodyneComponent::configureComponent(XmlComponentConfig config) 180 | { 181 | QString recordingParam = param.getProperty(kPropertyRecording.c_str()); 182 | if (!recordingParam.isNull()) { 183 | recording = recordingParam.toInt(); 184 | } 185 | LOG_INFO("property " << kPropertyRecording << "=\"" << recording << "\""); 186 | /* 187 | if (!param.getProperty("velodyneIP").isNull()) { 188 | host_ = param.getProperty("velodyneIP"); 189 | } 190 | if (!param.getProperty("velodynePort").isNull()) { 191 | port_ = param.getProperty("velodynePort").toInt(); 192 | } 193 | */ 194 | return ComponentBase::CONFIGURED_OK; 195 | } 196 | 197 | ////////////////////////////////////////////////////////////////////////// 198 | /// new data coming from Velodyne sensor 199 | void VelodyneComponent::readPendingDatagrams() 200 | { 201 | // get a timestamp 202 | road_time_t t = road_time(); 203 | if (mRunning) { 204 | while (mSocket->hasPendingDatagrams()) { 205 | QByteArray datagram; 206 | datagram.resize(mSocket->pendingDatagramSize()); 207 | mSocket->readDatagram(datagram.data(), datagram.size(), &mHost, &mPort); 208 | processTheDatagram(t, datagram); 209 | if (mEndOfScan) { 210 | // we have a complete scan of 360° so we can expose it the application 211 | exposeData(); 212 | if (recording) { 213 | record(); 214 | } 215 | mEndOfScan = false; 216 | } 217 | } 218 | } 219 | } 220 | 221 | ////////////////////////////////////////////////////////////////////////// 222 | /// TODO: doc 223 | void VelodyneComponent::processTheDatagram(road_time_t time, QByteArray data) 224 | { 225 | // envoi des paquets de 1206 octets 12 x 100 + 6 226 | // 12 fois : 227 | // 2 octets identifiant le bloc laser (haut ou bas) 228 | // 2 octets 0 35999, /100 => angle en degré 229 | // 96 octets : 32 laser beams, 2 octets distance 0.2 cm increment et 1 octet sur intensité 230 | // 6 octets : 0xhhhhDegC : température ou version firmware Vxxx 231 | 232 | const int packetSize = data.size(); 233 | 234 | // check the size of the packet 235 | if (packetSize != VELODYNE_PACKET_SIZE) { 236 | LOG_WARN("strange packet size:" 237 | << " EXPECTED = " << VELODYNE_PACKET_SIZE 238 | << " ACTUAL = " << packetSize 239 | ); 240 | return; 241 | } 242 | 243 | // check angle to know if we have done a complete revolution 244 | //LOG_TRACE("1st angle of packet = " << firstAngle); 245 | LOG_TRACE(data.data()); 246 | //LOG_TRACE(data.mid(2,2).toHex()); 247 | 248 | unsigned short scanCount; 249 | int angle; 250 | if (!mStartOfScan) { 251 | for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) { 252 | { 253 | bool ok = false; 254 | // for each 100 data bytes, we extract the corresponding azimuth angle 255 | angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16)); 256 | scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(&ok, 16)); 257 | LOG_TRACE("1:" << "start of scan = " << mStartOfScan << "\t" 258 | << "angle = " << angle << "\t" 259 | << "ok = " << ok << "\t" 260 | << "size = " << packetSize 261 | ); 262 | } 263 | int delta = angle - mPreviousAngle; 264 | LOG_TRACE("angle = " << angle); 265 | LOG_TRACE("delta = " << delta); 266 | LOG_TRACE("#scans = " << scanCount); 267 | 268 | if (delta < 0) { 269 | // we are looking for a new revolution 270 | mPreviousAngle = angle; 271 | mStartOfScan=true; 272 | mVelodyneData->time = time; 273 | 274 | int sizeToCopy = packetSize - i*VELODYNE_BLOCK_SIZE - 6; 275 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid(i*VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy); 276 | 277 | /* samuel // Copy the time in each blocks. 278 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - i; ++j) 279 | mVelodyneData->dataTime[mBlockIndex + j] = time; 280 | //*/ 281 | mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET - i; 282 | LOG_TRACE("block index = " << mBlockIndex); 283 | LOG_TRACE("#scans = " << scanCount); 284 | break; 285 | } else { 286 | mPreviousAngle = angle; 287 | } 288 | } 289 | } else { 290 | // start of scan 291 | int lastBlockIndex = 0; 292 | for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) { 293 | { 294 | bool ok = false; 295 | angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16)); 296 | scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(NULL, 16)); 297 | LOG_TRACE("2:" << "start of scan = " << mStartOfScan << "\t" 298 | << "angle = " << angle << "\t" 299 | << "ok = " << ok << "\t" 300 | << "size = " << packetSize 301 | ); 302 | } 303 | int delta = angle - mPreviousAngle; 304 | LOG_TRACE("angle = " << angle); 305 | LOG_TRACE("delta = " << delta); 306 | LOG_TRACE("#scans = " << scanCount); 307 | 308 | if (delta < 0) { 309 | // we are looking for a new revolution 310 | LOG_TRACE("block index = " << mBlockIndex); 311 | LOG_TRACE("#scans = " << scanCount); 312 | //char c; 313 | //cin >> c; 314 | mEndOfScan = true; 315 | // we add +1 because we detect the new revolution in the upper block and we have to copy the lower block too! 316 | lastBlockIndex = i+1; 317 | } 318 | mPreviousAngle = angle; 319 | } 320 | if (!mEndOfScan) { 321 | // we don't reach a complete revolution so only copy bytes in the current buffer 322 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.data(), VELODYNE_PACKET_SIZE - 6); 323 | /*samuel // Copy the time in each blocks. 324 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET; ++j) 325 | mVelodyneData->dataTime[mBlockIndex + j] = time; 326 | //*/ 327 | // Next offset. 328 | mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET; 329 | } else { 330 | // we have a complete revolution, we copy the starting data to the current buffer, then switch buffer 331 | // and copy the rest of datagram in the new buffer. 332 | if (lastBlockIndex > 1) { 333 | int sizeToCopy = packetSize - (VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1)) * VELODYNE_BLOCK_SIZE - 6; 334 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.left(sizeToCopy).data(), sizeToCopy); 335 | /* samuel // Copy the time in each blocks. 336 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1); ++j) 337 | mVelodyneData->dataTime[mBlockIndex + j] = time; 338 | //*/ 339 | } 340 | 341 | mVelodyneData->range = mBlockIndex+(lastBlockIndex - 1); 342 | LOG_DEBUG("range = " << mVelodyneData->range); 343 | mVelodyneData->timerange = time - mVelodyneData->time; 344 | mBlockIndex = 0; 345 | switchBuffer(); // switch the circular buffer of data. Previous one will be exported later. 346 | 347 | // copy the rest of incoming data in the new buffer 348 | mVelodyneData->time = time; 349 | int sizeToCopy = packetSize - (lastBlockIndex - 1)*VELODYNE_BLOCK_SIZE - 6; 350 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid((lastBlockIndex-1) * VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy); 351 | /* samuel // Copy the time in each blocks. 352 | for (size_t j = 0; j < lastBlockIndex - 1; ++j) 353 | mVelodyneData->dataTime[mBlockIndex + j] = time; 354 | //*/ 355 | mBlockIndex += sizeToCopy / VELODYNE_BLOCK_SIZE; 356 | } 357 | } 358 | } 359 | 360 | /// TODO: doc 361 | void VelodyneComponent::record() 362 | { 363 | LOG_TRACE("record()"); 364 | 365 | //qDebug() << fullBuffer_ << velodyneData_<< &(velodyneDataBuffer_[0]) << &(velodyneDataBuffer_[1]); 366 | /*LOG_DEBUG(mFullBuffer->time); 367 | LOG_DEBUG(mFullBuffer->timerange); 368 | LOG_DEBUG((intptr_t)mFullBuffer); 369 | LOG_DEBUG((intptr_t)mVelodyneData); 370 | LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[0])); 371 | LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[1]));*/ 372 | if (recording) { 373 | size_t dataSize = sizeof(VelodynePolarData); 374 | mVelodyneSphericDataFile.writeRecord(mFullBuffer->time, mFullBuffer->timerange, (const char *) mFullBuffer, dataSize); 375 | } 376 | 377 | LOG_INFO("record succeed"); 378 | } 379 | 380 | /// TODO: doc 381 | void VelodyneComponent::switchBuffer() 382 | { 383 | mFullBuffer = mVelodyneData; 384 | /* 385 | LOG_DEBUG("full buffer = " << mFullBuffer); 386 | LOG_DEBUG("data = " << mVelodyneData); 387 | LOG_DEBUG("current data = " << mVelodyneData);*/ 388 | mCurrentVelodyneData = (mCurrentVelodyneData+1)%2; // a hack to switch between 0 & 1 389 | //LOG_DEBUG("current data = " << mVelodyneData); 390 | // switch circular buffer 391 | mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]); 392 | } 393 | 394 | /// TODO: doc 395 | void VelodyneComponent::exposeData() 396 | { 397 | mShMem->write(mFullBuffer, sizeof(VelodynePolarData) ); 398 | } 399 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/text-base/VelodyneComponent.h.svn-base: -------------------------------------------------------------------------------- 1 | /// @date created 2010/03/26 - 14:30 2 | /// @author Gerald Dherbomez 3 | /// @copyright Heudiasyc UMR UTC/CNRS 6599 4 | /// @version $Id: $ 5 | 6 | #ifndef VELODYNECOMPONENT_H 7 | #define VELODYNECOMPONENT_H 8 | 9 | #include 10 | #include 11 | 12 | #include "kernel/ComponentBase.h" 13 | #include "kernel/DbiteFile.h" 14 | #include "kernel/road_time.h" 15 | #include "PacpusTools/ShMem.h" 16 | #include "structure/structure_velodyne.h" 17 | 18 | // TODO ! 19 | // faire une classe VelodyneDecoding qui s'occupera de traiter les données en provenance du slot readPendingDatagrams 20 | // en effet, la classe VelodyneComponent vit dans le thread GUI alors que le socket udp vit dans le thread de VelodyneComponent 21 | // du coup la connexion signal/slot n'est pas directe et mise en file d'attente. Il semble qu'il y ait ensuite un accès concurrent 22 | // quelque part qui fait planter le programme. 23 | // pas de plantage en mode monothread ! 24 | // cf. discussion http://lists.trolltech.com/qt-interest/2007-01/msg00757.html qui donne des infos et détails du même type 25 | // rem : la classe VelodyneDecoding devra être instancier dans le run() de VelodyneComponent pour que la connexion 26 | // se fassent entre des objets ayant le même contexte de thread ! 27 | 28 | // Export macro for AlascaXT DLL for Windows only 29 | #ifdef WIN32 30 | # ifdef VELODYNEHDL64S2_EXPORTS 31 | // make DLL 32 | # define VELODYNEHDL64S2_API __declspec(dllexport) 33 | # else 34 | // use DLL 35 | # define VELODYNEHDL64S2_API __declspec(dllimport) 36 | # endif 37 | #else 38 | // On other platforms, simply ignore this 39 | # define VELODYNEHDL64S2_API 40 | #endif 41 | 42 | class QUdpSocket; 43 | 44 | namespace pacpus { 45 | 46 | class VELODYNEHDL64S2_API VelodyneComponent 47 | : public QThread 48 | , public ComponentBase 49 | { 50 | Q_OBJECT 51 | 52 | public: 53 | VelodyneComponent(QString name); 54 | ~VelodyneComponent(); 55 | 56 | /// to stop the processing thread 57 | virtual void stopActivity(); 58 | /// to start the processing thread 59 | virtual void startActivity(); 60 | virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 61 | 62 | public Q_SLOTS: 63 | void readPendingDatagrams(); 64 | 65 | protected: 66 | void initialize(); 67 | void close(); 68 | void initSocket(); 69 | void closeSocket(); 70 | void run(); 71 | void processTheDatagram(road_time_t time, QByteArray data); 72 | void record(); 73 | void exposeData(); 74 | void switchBuffer(); 75 | 76 | private: 77 | QUdpSocket * mSocket; 78 | 79 | bool mStartOfScan, mEndOfScan; 80 | unsigned short mBlockIndex; 81 | 82 | /// The Velodyne IP or hostname 83 | QHostAddress mHost; 84 | 85 | /// The Velodyne port 86 | quint16 mPort; 87 | 88 | struct VelodynePolarData mVelodyneDataBuffer[2]; 89 | struct VelodynePolarData * mVelodyneData; // a pointer to the current velodyneDataBuffer_ 90 | struct VelodynePolarData * mFullBuffer; // a pointer to the velodyne data which is completly filled 91 | 92 | int mCurrentVelodyneData; 93 | int mPreviousAngle; 94 | bool mRunning; 95 | 96 | DbiteFile mVelodyneSphericDataFile; 97 | 98 | ShMem * mShMem; 99 | }; 100 | 101 | } // namespace pacpus 102 | 103 | #endif // VELODYNECOMPONENT_H 104 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/.svn/text-base/VelodyneViewer.h.svn-base: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/03/26 - 14:30 3 | // filename: VelodyneComponent.h 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id: $ 9 | // 10 | // purpose: 11 | *********************************************************************/ 12 | 13 | 14 | 15 | #ifndef _VELODYNECOMPONENT_H_ 16 | #define _VELODYNECOMPONENT_H_ 17 | 18 | #include 19 | #include 20 | #include "kernel/road_time.h" 21 | #include "PacpusTools/ShMem.h" 22 | #include "kernel/ComponentBase.h" 23 | #include "structure/structure_velodyne.h" 24 | 25 | 26 | 27 | 28 | class VelodyneComponent : public QThread, public ComponentBase 29 | { 30 | Q_OBJECT 31 | 32 | public: 33 | VelodyneComponent(QString name); 34 | ~VelodyneComponent(); 35 | 36 | virtual void stopActivity(); /*!< to stop the processing thread */ 37 | virtual void startActivity(); /*!< to start the processing thread */ 38 | virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 39 | 40 | public slots: 41 | void readPendingDatagrams(); 42 | 43 | protected: 44 | void initSocket(); 45 | void closeSocket(); 46 | void run(); 47 | void processTheDatagram(road_time_t time, QByteArray data); 48 | void record(); 49 | void exposeData(); 50 | void switchBuffer(); 51 | 52 | 53 | private: 54 | QUdpSocket *socket_; 55 | 56 | bool startOfScan_, endOfScan_; 57 | unsigned short idxBlock_; 58 | // The Velodyne IP or hostname 59 | QHostAddress host_; 60 | // The Velodyne port 61 | quint16 port_; 62 | 63 | struct VelodynePolarData velodyneDataBuffer_[2]; 64 | struct VelodynePolarData *velodyneData_; // a pointer to the current velodyneDataBuffer_ 65 | struct VelodynePolarData *fullBuffer_; // a pointer to the velodyne data which is completly filled 66 | 67 | int currentVelodyneData_; 68 | 69 | int previousAngle_; 70 | 71 | bool running_; 72 | 73 | // dbt files 74 | hdfile_t * velodyneSphericDataFile_; 75 | 76 | }; 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(VelodyneComponent) 2 | 3 | ################################################################################ 4 | add_definitions( -DVELODYNEHDL64S2_EXPORTS ) 5 | 6 | # ======================================== 7 | # Configure qt4 8 | # ======================================== 9 | if(QT4_FOUND) 10 | set(QT_USE_QTXML true) 11 | set(QT_USE_QTNETWORK true) 12 | include(${QT_USE_FILE}) 13 | else() 14 | message(ERROR "Qt4 needed") 15 | endif() 16 | 17 | # ======================================== 18 | # Compiler definitions 19 | # ======================================== 20 | add_definitions( 21 | ${QT_DEFINITIONS} 22 | ) 23 | 24 | # ======================================== 25 | # Include directories 26 | # ======================================== 27 | include_directories( 28 | ${PROJECT_BINARY_DIR} 29 | ${QT_INCLUDE_DIR} 30 | ) 31 | 32 | # ======================================== 33 | # Link directories 34 | # ======================================== 35 | link_directories( ${PACPUS_LIB_DIR} 36 | ) 37 | 38 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME} ) 39 | 40 | set(HDRS 41 | VelodyneComponent.h 42 | ) 43 | 44 | 45 | # ======================================== 46 | # List of sources 47 | # ======================================== 48 | set( 49 | PROJECT_SRCS 50 | VelodyneComponent.cpp 51 | ${HDRS} 52 | ${PLUGIN_CPP} 53 | ) 54 | 55 | # ======================================== 56 | # Files to MOC 57 | # ======================================== 58 | set( 59 | FILES_TO_MOC 60 | VelodyneComponent.h 61 | ${PLUGIN_H} 62 | ) 63 | 64 | 65 | set( 66 | UI_FILES 67 | 68 | ) 69 | 70 | # ======================================== 71 | # Call MOC 72 | # ======================================== 73 | qt4_wrap_cpp( 74 | PROJECT_MOC_SRCS 75 | ${FILES_TO_MOC} 76 | ) 77 | 78 | qt4_wrap_ui( 79 | PROJECT_UI_SRCS 80 | ${UI_FILES} 81 | ) 82 | 83 | # ======================================== 84 | # Build a library 85 | # ======================================== 86 | pacpus_add_library( 87 | ${PROJECT_NAME} SHARED 88 | ${PROJECT_SRCS} 89 | ${PROJECT_MOC_SRCS} 90 | ${PROJECT_UI_SRCS} 91 | ) 92 | 93 | set(LIBS 94 | optimized FileLib debug FileLib_d 95 | optimized PacpusLib debug PacpusLib_d 96 | optimized PacpusTools debug PacpusTools_d 97 | ) 98 | if (WIN32) 99 | list(APPEND LIBS 100 | optimized ROAD_TIME debug ROAD_TIME_d 101 | ) 102 | endif() 103 | 104 | # ======================================== 105 | # Libraries 106 | # ======================================== 107 | # All the platform 108 | target_link_libraries( 109 | ${PROJECT_NAME} 110 | ${PACPUS_LIBRARIES} 111 | ${QT_LIBRARIES} 112 | ${PACPUS_DEPENDENCIES_LIB} 113 | ${LIBS} 114 | ) 115 | 116 | pacpus_folder(${PROJECT_NAME} "components") 117 | 118 | # ======================================== 119 | # Install 120 | # ======================================== 121 | pacpus_install(${PROJECT_NAME}) 122 | 123 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneComponent.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/03/26 - 14:38 3 | // filename: VelodyneComponent.cpp 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id: $ 9 | // 10 | // purpose: The acquisition component of the Velodyne HDL64S2 sensor 11 | // 12 | *********************************************************************/ 13 | 14 | #include "VelodyneComponent.h" 15 | 16 | #include "kernel/cstdint.h" 17 | #include "kernel/ComponentFactory.h" 18 | #include "kernel/DbiteFileTypes.h" 19 | #include "kernel/Log.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace pacpus; 26 | using namespace std; 27 | 28 | DECLARE_STATIC_LOGGER("pacpus.base.VelodyneComponent"); 29 | 30 | /// Construct the factory 31 | static ComponentFactory sFactory("VelodyneComponent"); 32 | 33 | /// Default host address = 0.0.0.0 (any address) 34 | static const string kDefaultHostAddress = "0.0.0.0"; 35 | 36 | /// Default host port = 2368 37 | static const uint16_t kDefaultHostPort = 2368; 38 | 39 | static const string kPropertyRecording = "recording"; 40 | 41 | static const string kVelodyneSharedMemoryName = "VELODYNE"; 42 | static const string kDefaultOutputFilename = "velodyne_spheric.dbt"; 43 | 44 | ////////////////////////////////////////////////////////////////////////// 45 | /// Constructor 46 | VelodyneComponent::VelodyneComponent(QString name) 47 | : ComponentBase(name) 48 | , mPort(kDefaultHostPort) 49 | { 50 | LOG_TRACE("constructor(" << name << ")"); 51 | 52 | ComponentManager * mgr = ComponentManager::getInstance(); 53 | LOG_DEBUG("manager = " << mgr); 54 | 55 | recording = true; 56 | 57 | // default values 58 | if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) { 59 | LOG_ERROR("cannot set address"); 60 | } 61 | LOG_DEBUG("host address = " << mHost.toString()); 62 | } 63 | 64 | ////////////////////////////////////////////////////////////////////////// 65 | /// Destructor 66 | VelodyneComponent::~VelodyneComponent() 67 | { 68 | } 69 | 70 | ////////////////////////////////////////////////////////////////////////// 71 | /// Called by the ComponentManager to start the component 72 | void VelodyneComponent::startActivity() 73 | { 74 | mStartOfScan = false; 75 | mEndOfScan = false; 76 | mBlockIndex = 0; 77 | mCurrentVelodyneData = 0; 78 | mPreviousAngle = 0; 79 | mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]); 80 | 81 | mRunning = true; 82 | initialize(); 83 | } 84 | 85 | ////////////////////////////////////////////////////////////////////////// 86 | /// TODO: doc 87 | void VelodyneComponent::run() 88 | { 89 | initialize(); 90 | exec(); // launch the exec loop, blocking until receiving exit() signal ... 91 | close(); 92 | } 93 | 94 | ////////////////////////////////////////////////////////////////////////// 95 | /// TODO: doc 96 | void VelodyneComponent::initialize() 97 | { 98 | if (recording) { 99 | mVelodyneSphericDataFile.open(kDefaultOutputFilename, WriteMode, VELODYNE_RAW_SPHERIC_DATA, sizeof(VelodynePolarData)); 100 | } 101 | 102 | initSocket(); 103 | if (!connect(mSocket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()))) { 104 | LOG_ERROR("cannot connect SIGNAL(readyRead()) to SLOT(readPendingDatagrams())"); 105 | } 106 | 107 | mShMem = new ShMem(kVelodyneSharedMemoryName.c_str(), sizeof(VelodynePolarData) ); 108 | if (!mShMem) { 109 | LOG_FATAL("cannot create Velodyne shared memory"); 110 | return; 111 | } 112 | } 113 | 114 | ////////////////////////////////////////////////////////////////////////// 115 | /// TODO: doc 116 | void VelodyneComponent::initSocket() 117 | { 118 | if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) { 119 | LOG_FATAL("failed to set address"); 120 | } 121 | mPort = kDefaultHostPort; 122 | mSocket = new QUdpSocket(); 123 | if (!mSocket) { 124 | LOG_FATAL("cannot create socket"); 125 | return; 126 | } 127 | if (!(mSocket->bind(mHost, mPort))) { 128 | LOG_ERROR("error when binding velodyne"); 129 | } 130 | } 131 | 132 | ////////////////////////////////////////////////////////////////////////// 133 | /// Called by the ComponentManager to stop the component 134 | void VelodyneComponent::stopActivity() 135 | { 136 | mRunning = false; 137 | /* 138 | exit(); 139 | if (!wait(2000)) { 140 | // wait termination during 2 seconds 141 | LOG_ERROR("thread blocking, terminate it"); 142 | terminate(); 143 | } 144 | */ 145 | 146 | close(); 147 | } 148 | 149 | ////////////////////////////////////////////////////////////////////////// 150 | /// TODO: doc 151 | void VelodyneComponent::close() 152 | { 153 | closeSocket(); 154 | 155 | if (recording) { 156 | mVelodyneSphericDataFile.close(); 157 | } 158 | 159 | if (mShMem) { 160 | delete mShMem; 161 | mShMem = NULL; 162 | } 163 | } 164 | 165 | ////////////////////////////////////////////////////////////////////////// 166 | /// TODO: doc 167 | void VelodyneComponent::closeSocket() 168 | { 169 | if (mSocket) { 170 | mSocket->close(); 171 | delete mSocket; 172 | mSocket = NULL; 173 | } 174 | } 175 | 176 | ////////////////////////////////////////////////////////////////////////// 177 | /// Called by the ComponentManager to pass the XML parameters to the 178 | /// component 179 | ComponentBase::COMPONENT_CONFIGURATION VelodyneComponent::configureComponent(XmlComponentConfig config) 180 | { 181 | QString recordingParam = param.getProperty(kPropertyRecording.c_str()); 182 | if (!recordingParam.isNull()) { 183 | recording = recordingParam.toInt(); 184 | } 185 | LOG_INFO("property " << kPropertyRecording << "=\"" << recording << "\""); 186 | /* 187 | if (!param.getProperty("velodyneIP").isNull()) { 188 | host_ = param.getProperty("velodyneIP"); 189 | } 190 | if (!param.getProperty("velodynePort").isNull()) { 191 | port_ = param.getProperty("velodynePort").toInt(); 192 | } 193 | */ 194 | return ComponentBase::CONFIGURED_OK; 195 | } 196 | 197 | ////////////////////////////////////////////////////////////////////////// 198 | /// new data coming from Velodyne sensor 199 | void VelodyneComponent::readPendingDatagrams() 200 | { 201 | // get a timestamp 202 | road_time_t t = road_time(); 203 | if (mRunning) { 204 | while (mSocket->hasPendingDatagrams()) { 205 | QByteArray datagram; 206 | datagram.resize(mSocket->pendingDatagramSize()); 207 | mSocket->readDatagram(datagram.data(), datagram.size(), &mHost, &mPort); 208 | processTheDatagram(t, datagram); 209 | if (mEndOfScan) { 210 | // we have a complete scan of 360° so we can expose it the application 211 | exposeData(); 212 | if (recording) { 213 | record(); 214 | } 215 | mEndOfScan = false; 216 | } 217 | } 218 | } 219 | } 220 | 221 | ////////////////////////////////////////////////////////////////////////// 222 | /// TODO: doc 223 | void VelodyneComponent::processTheDatagram(road_time_t time, QByteArray data) 224 | { 225 | // envoi des paquets de 1206 octets 12 x 100 + 6 226 | // 12 fois : 227 | // 2 octets identifiant le bloc laser (haut ou bas) 228 | // 2 octets 0 35999, /100 => angle en degré 229 | // 96 octets : 32 laser beams, 2 octets distance 0.2 cm increment et 1 octet sur intensité 230 | // 6 octets : 0xhhhhDegC : température ou version firmware Vxxx 231 | 232 | const int packetSize = data.size(); 233 | 234 | // check the size of the packet 235 | if (packetSize != VELODYNE_PACKET_SIZE) { 236 | LOG_WARN("strange packet size:" 237 | << " EXPECTED = " << VELODYNE_PACKET_SIZE 238 | << " ACTUAL = " << packetSize 239 | ); 240 | return; 241 | } 242 | 243 | // check angle to know if we have done a complete revolution 244 | //LOG_TRACE("1st angle of packet = " << firstAngle); 245 | LOG_TRACE(data.data()); 246 | //LOG_TRACE(data.mid(2,2).toHex()); 247 | 248 | unsigned short scanCount; 249 | int angle; 250 | if (!mStartOfScan) { 251 | for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) { 252 | { 253 | bool ok = false; 254 | // for each 100 data bytes, we extract the corresponding azimuth angle 255 | angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16)); 256 | scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(&ok, 16)); 257 | LOG_TRACE("1:" << "start of scan = " << mStartOfScan << "\t" 258 | << "angle = " << angle << "\t" 259 | << "ok = " << ok << "\t" 260 | << "size = " << packetSize 261 | ); 262 | } 263 | int delta = angle - mPreviousAngle; 264 | LOG_TRACE("angle = " << angle); 265 | LOG_TRACE("delta = " << delta); 266 | LOG_TRACE("#scans = " << scanCount); 267 | 268 | if (delta < 0) { 269 | // we are looking for a new revolution 270 | mPreviousAngle = angle; 271 | mStartOfScan=true; 272 | mVelodyneData->time = time; 273 | 274 | int sizeToCopy = packetSize - i*VELODYNE_BLOCK_SIZE - 6; 275 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid(i*VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy); 276 | 277 | /* samuel // Copy the time in each blocks. 278 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - i; ++j) 279 | mVelodyneData->dataTime[mBlockIndex + j] = time; 280 | //*/ 281 | mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET - i; 282 | LOG_TRACE("block index = " << mBlockIndex); 283 | LOG_TRACE("#scans = " << scanCount); 284 | break; 285 | } else { 286 | mPreviousAngle = angle; 287 | } 288 | } 289 | } else { 290 | // start of scan 291 | int lastBlockIndex = 0; 292 | for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) { 293 | { 294 | bool ok = false; 295 | angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16)); 296 | scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(NULL, 16)); 297 | LOG_TRACE("2:" << "start of scan = " << mStartOfScan << "\t" 298 | << "angle = " << angle << "\t" 299 | << "ok = " << ok << "\t" 300 | << "size = " << packetSize 301 | ); 302 | } 303 | int delta = angle - mPreviousAngle; 304 | LOG_TRACE("angle = " << angle); 305 | LOG_TRACE("delta = " << delta); 306 | LOG_TRACE("#scans = " << scanCount); 307 | 308 | if (delta < 0) { 309 | // we are looking for a new revolution 310 | LOG_TRACE("block index = " << mBlockIndex); 311 | LOG_TRACE("#scans = " << scanCount); 312 | //char c; 313 | //cin >> c; 314 | mEndOfScan = true; 315 | // we add +1 because we detect the new revolution in the upper block and we have to copy the lower block too! 316 | lastBlockIndex = i+1; 317 | } 318 | mPreviousAngle = angle; 319 | } 320 | if (!mEndOfScan) { 321 | // we don't reach a complete revolution so only copy bytes in the current buffer 322 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.data(), VELODYNE_PACKET_SIZE - 6); 323 | /*samuel // Copy the time in each blocks. 324 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET; ++j) 325 | mVelodyneData->dataTime[mBlockIndex + j] = time; 326 | //*/ 327 | // Next offset. 328 | mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET; 329 | } else { 330 | // we have a complete revolution, we copy the starting data to the current buffer, then switch buffer 331 | // and copy the rest of datagram in the new buffer. 332 | if (lastBlockIndex > 1) { 333 | int sizeToCopy = packetSize - (VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1)) * VELODYNE_BLOCK_SIZE - 6; 334 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.left(sizeToCopy).data(), sizeToCopy); 335 | /* samuel // Copy the time in each blocks. 336 | for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1); ++j) 337 | mVelodyneData->dataTime[mBlockIndex + j] = time; 338 | //*/ 339 | } 340 | 341 | mVelodyneData->range = mBlockIndex+(lastBlockIndex - 1); 342 | LOG_DEBUG("range = " << mVelodyneData->range); 343 | mVelodyneData->timerange = time - mVelodyneData->time; 344 | mBlockIndex = 0; 345 | switchBuffer(); // switch the circular buffer of data. Previous one will be exported later. 346 | 347 | // copy the rest of incoming data in the new buffer 348 | mVelodyneData->time = time; 349 | int sizeToCopy = packetSize - (lastBlockIndex - 1)*VELODYNE_BLOCK_SIZE - 6; 350 | memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid((lastBlockIndex-1) * VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy); 351 | /* samuel // Copy the time in each blocks. 352 | for (size_t j = 0; j < lastBlockIndex - 1; ++j) 353 | mVelodyneData->dataTime[mBlockIndex + j] = time; 354 | //*/ 355 | mBlockIndex += sizeToCopy / VELODYNE_BLOCK_SIZE; 356 | } 357 | } 358 | } 359 | 360 | /// TODO: doc 361 | void VelodyneComponent::record() 362 | { 363 | LOG_TRACE("record()"); 364 | 365 | //qDebug() << fullBuffer_ << velodyneData_<< &(velodyneDataBuffer_[0]) << &(velodyneDataBuffer_[1]); 366 | /*LOG_DEBUG(mFullBuffer->time); 367 | LOG_DEBUG(mFullBuffer->timerange); 368 | LOG_DEBUG((intptr_t)mFullBuffer); 369 | LOG_DEBUG((intptr_t)mVelodyneData); 370 | LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[0])); 371 | LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[1]));*/ 372 | if (recording) { 373 | size_t dataSize = sizeof(VelodynePolarData); 374 | mVelodyneSphericDataFile.writeRecord(mFullBuffer->time, mFullBuffer->timerange, (const char *) mFullBuffer, dataSize); 375 | } 376 | 377 | LOG_INFO("record succeed"); 378 | } 379 | 380 | /// TODO: doc 381 | void VelodyneComponent::switchBuffer() 382 | { 383 | mFullBuffer = mVelodyneData; 384 | /* 385 | LOG_DEBUG("full buffer = " << mFullBuffer); 386 | LOG_DEBUG("data = " << mVelodyneData); 387 | LOG_DEBUG("current data = " << mVelodyneData);*/ 388 | mCurrentVelodyneData = (mCurrentVelodyneData+1)%2; // a hack to switch between 0 & 1 389 | //LOG_DEBUG("current data = " << mVelodyneData); 390 | // switch circular buffer 391 | mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]); 392 | } 393 | 394 | /// TODO: doc 395 | void VelodyneComponent::exposeData() 396 | { 397 | mShMem->write(mFullBuffer, sizeof(VelodynePolarData) ); 398 | } 399 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneComponent.h: -------------------------------------------------------------------------------- 1 | /// @date created 2010/03/26 - 14:30 2 | /// @author Gerald Dherbomez 3 | /// @copyright Heudiasyc UMR UTC/CNRS 6599 4 | /// @version $Id: $ 5 | 6 | #ifndef VELODYNECOMPONENT_H 7 | #define VELODYNECOMPONENT_H 8 | 9 | #include 10 | #include 11 | 12 | #include "kernel/ComponentBase.h" 13 | #include "kernel/DbiteFile.h" 14 | #include "kernel/road_time.h" 15 | #include "PacpusTools/ShMem.h" 16 | #include "structure_velodyne.h" 17 | 18 | // TODO ! 19 | // faire une classe VelodyneDecoding qui s'occupera de traiter les données en provenance du slot readPendingDatagrams 20 | // en effet, la classe VelodyneComponent vit dans le thread GUI alors que le socket udp vit dans le thread de VelodyneComponent 21 | // du coup la connexion signal/slot n'est pas directe et mise en file d'attente. Il semble qu'il y ait ensuite un accès concurrent 22 | // quelque part qui fait planter le programme. 23 | // pas de plantage en mode monothread ! 24 | // cf. discussion http://lists.trolltech.com/qt-interest/2007-01/msg00757.html qui donne des infos et détails du même type 25 | // rem : la classe VelodyneDecoding devra être instancier dans le run() de VelodyneComponent pour que la connexion 26 | // se fassent entre des objets ayant le même contexte de thread ! 27 | 28 | // Export macro for AlascaXT DLL for Windows only 29 | #ifdef WIN32 30 | # ifdef VELODYNEHDL64S2_EXPORTS 31 | // make DLL 32 | # define VELODYNEHDL64S2_API __declspec(dllexport) 33 | # else 34 | // use DLL 35 | # define VELODYNEHDL64S2_API __declspec(dllimport) 36 | # endif 37 | #else 38 | // On other platforms, simply ignore this 39 | # define VELODYNEHDL64S2_API 40 | #endif 41 | 42 | class QUdpSocket; 43 | 44 | namespace pacpus { 45 | 46 | class VELODYNEHDL64S2_API VelodyneComponent 47 | : public QThread 48 | , public ComponentBase 49 | { 50 | Q_OBJECT 51 | 52 | public: 53 | VelodyneComponent(QString name); 54 | ~VelodyneComponent(); 55 | 56 | /// to stop the processing thread 57 | virtual void stopActivity(); 58 | /// to start the processing thread 59 | virtual void startActivity(); 60 | virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 61 | 62 | public Q_SLOTS: 63 | void readPendingDatagrams(); 64 | 65 | protected: 66 | void initialize(); 67 | void close(); 68 | void initSocket(); 69 | void closeSocket(); 70 | void run(); 71 | void processTheDatagram(road_time_t time, QByteArray data); 72 | void record(); 73 | void exposeData(); 74 | void switchBuffer(); 75 | 76 | private: 77 | QUdpSocket * mSocket; 78 | 79 | bool mStartOfScan, mEndOfScan; 80 | unsigned short mBlockIndex; 81 | 82 | /// The Velodyne IP or hostname 83 | QHostAddress mHost; 84 | 85 | /// The Velodyne port 86 | quint16 mPort; 87 | 88 | struct VelodynePolarData mVelodyneDataBuffer[2]; 89 | struct VelodynePolarData * mVelodyneData; // a pointer to the current velodyneDataBuffer_ 90 | struct VelodynePolarData * mFullBuffer; // a pointer to the velodyne data which is completly filled 91 | 92 | int mCurrentVelodyneData; 93 | int mPreviousAngle; 94 | bool mRunning; 95 | 96 | DbiteFile mVelodyneSphericDataFile; 97 | 98 | ShMem * mShMem; 99 | }; 100 | 101 | } // namespace pacpus 102 | 103 | #endif // VELODYNECOMPONENT_H 104 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneComponentPlugin.cpp: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #include "VelodyneComponentPlugin.h" 5 | 6 | VelodyneComponentPlugin::VelodyneComponentPlugin() 7 | { 8 | } 9 | 10 | VelodyneComponentPlugin::~VelodyneComponentPlugin() 11 | { 12 | } 13 | 14 | QString VelodyneComponentPlugin::name() 15 | { 16 | return "VelodyneComponentPlugin"; 17 | } 18 | 19 | Q_EXPORT_PLUGIN2(VelodyneComponentPlugin, VelodyneComponentPlugin) 20 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneComponentPlugin.h: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #ifndef __VELODYNECOMPONENTPLUGIN_H__ 5 | #define __VELODYNECOMPONENTPLUGIN_H__ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /// Auto-generated plugin class 13 | class VelodyneComponentPlugin 14 | : public QObject 15 | , public PacpusPluginInterface 16 | { 17 | Q_OBJECT 18 | Q_INTERFACES(PacpusPluginInterface) 19 | 20 | public: 21 | VelodyneComponentPlugin(); 22 | ~VelodyneComponentPlugin(); 23 | 24 | protected: 25 | QString name(); 26 | }; 27 | 28 | #endif // __VELODYNECOMPONENTPLUGIN_H__ 29 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneHDL64S2Plugin.cpp: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #include "VelodyneHDL64S2Plugin.h" 5 | 6 | VelodyneHDL64S2Plugin::VelodyneHDL64S2Plugin() 7 | { 8 | } 9 | 10 | VelodyneHDL64S2Plugin::~VelodyneHDL64S2Plugin() 11 | { 12 | } 13 | 14 | QString VelodyneHDL64S2Plugin::name() 15 | { 16 | return "VelodyneHDL64S2Plugin"; 17 | } 18 | 19 | Q_EXPORT_PLUGIN2(VelodyneHDL64S2Plugin, VelodyneHDL64S2Plugin) 20 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneHDL64S2Plugin.h: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #ifndef __VELODYNEHDL64S2PLUGIN_H__ 5 | #define __VELODYNEHDL64S2PLUGIN_H__ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /// Auto-generated plugin class 13 | class VelodyneHDL64S2Plugin 14 | : public QObject 15 | , public PacpusPluginInterface 16 | { 17 | Q_OBJECT 18 | Q_INTERFACES(PacpusPluginInterface) 19 | 20 | public: 21 | VelodyneHDL64S2Plugin(); 22 | ~VelodyneHDL64S2Plugin(); 23 | 24 | protected: 25 | QString name(); 26 | }; 27 | 28 | #endif // __VELODYNEHDL64S2PLUGIN_H__ 29 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/VelodyneViewer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | // created: 2010/03/26 - 14:30 3 | // filename: VelodyneComponent.h 4 | // 5 | // author: Gerald Dherbomez 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599 7 | // 8 | // version: $Id: $ 9 | // 10 | // purpose: 11 | *********************************************************************/ 12 | 13 | 14 | 15 | #ifndef _VELODYNECOMPONENT_H_ 16 | #define _VELODYNECOMPONENT_H_ 17 | 18 | #include 19 | #include 20 | #include "kernel/road_time.h" 21 | #include "PacpusTools/ShMem.h" 22 | #include "kernel/ComponentBase.h" 23 | #include "structure_velodyne.h" 24 | 25 | 26 | 27 | 28 | class VelodyneComponent : public QThread, public ComponentBase 29 | { 30 | Q_OBJECT 31 | 32 | public: 33 | VelodyneComponent(QString name); 34 | ~VelodyneComponent(); 35 | 36 | virtual void stopActivity(); /*!< to stop the processing thread */ 37 | virtual void startActivity(); /*!< to start the processing thread */ 38 | virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 39 | 40 | public slots: 41 | void readPendingDatagrams(); 42 | 43 | protected: 44 | void initSocket(); 45 | void closeSocket(); 46 | void run(); 47 | void processTheDatagram(road_time_t time, QByteArray data); 48 | void record(); 49 | void exposeData(); 50 | void switchBuffer(); 51 | 52 | 53 | private: 54 | QUdpSocket *socket_; 55 | 56 | bool startOfScan_, endOfScan_; 57 | unsigned short idxBlock_; 58 | // The Velodyne IP or hostname 59 | QHostAddress host_; 60 | // The Velodyne port 61 | quint16 port_; 62 | 63 | struct VelodynePolarData velodyneDataBuffer_[2]; 64 | struct VelodynePolarData *velodyneData_; // a pointer to the current velodyneDataBuffer_ 65 | struct VelodynePolarData *fullBuffer_; // a pointer to the velodyne data which is completly filled 66 | 67 | int currentVelodyneData_; 68 | 69 | int previousAngle_; 70 | 71 | bool running_; 72 | 73 | // dbt files 74 | hdfile_t * velodyneSphericDataFile_; 75 | 76 | }; 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /pacpussensors/VelodyneComponent/structure_velodyne.h: -------------------------------------------------------------------------------- 1 | #ifndef STRUCTURE_VELODYNE_H 2 | #define STRUCTURE_VELODYNE_H 3 | 4 | // VELODYNE_BLOCK_SIZE = sizeof(unsigned short = uint16) * 2 + sizeof(VelodyneRawPoint = uint16,uint8) * kVelodynePointsPerBlock = 2*2 + (2+1)*32 = 4 + 96 = 100 5 | #define VELODYNE_BLOCK_SIZE 100 6 | // FIXME: unused 7 | // VELODYNE_PACKET_SIZE = 8 | #define VELODYNE_PACKET_SIZE 1206 9 | #define VELODYNE_SCAN_SIZE 4166 10 | #define VELODYNE_NB_BLOCKS_PER_PACKET 12 11 | 12 | #define kVelodyneUpperBlock 0xEEFF 13 | #define kVelodyneLowerBlock 0xDDFF 14 | #define kVelodynePointsPerBlock 32 15 | 16 | #pragma pack(push, 1) 17 | 18 | // 3 bytes size 19 | typedef struct VelodyneRawPoint 20 | { 21 | uint16_t distance; // 0.2cm increments - if 0 no return up to 65m 22 | uint8_t intensity; // 255 most intense return 23 | } VelodyneRawPoint; 24 | 25 | // 100=VELODYNE_BLOCK_SIZE bytes size 26 | typedef struct VelodyneBlock 27 | { 28 | /// 0xEEFF upper and 0xDDFF lower 29 | uint16_t block; 30 | 31 | /// azimuth in 100th of degrees [0-35999] 32 | uint16_t angle; 33 | 34 | /// a couple of distance and intensity representing a point 35 | struct VelodyneRawPoint rawPoints[kVelodynePointsPerBlock]; 36 | 37 | /* FIXME, block pacpus sensor with these 6 more bytes 38 | // change VELODYNE_BLOCK_SIZE to 106 after 39 | // Status (see p. 29 Rev C 2011). 40 | uint32_t timestamp; // 10e-06. 41 | uint8_t code; // ASCII code. 42 | uint8_t codeValue; // Value of the code, see the Rev C. 43 | */ 44 | } VelodyneBlock; 45 | 46 | // size : VELODYNE_BLOCK_SIZE*VELODYNE_SCAN_SIZE + sizeof(unsigned short = uint16)*VELODYNE_SCAN_SIZE 47 | // + sizeof(road_time_t = unsigned long long = uint64) + sizeof(road_timerange_t = int = int32) + sizeof(short = int16) 48 | // = 100*4166 + 2*4166 + 8 + 4 + 2 = 424 946 bytes 49 | // structure containing data of a complete revolution of the lidar 50 | typedef struct VelodynePolarData 51 | { 52 | /// data of one block of 32 beams (upper or lower) 53 | VelodyneBlock polarData[VELODYNE_SCAN_SIZE]; 54 | 55 | /// scan count relative to polarData above 56 | uint16_t scanCount[VELODYNE_SCAN_SIZE]; 57 | 58 | /// Timestamp of each block. 59 | //road_time_t dataTime[VELODYNE_SCAN_SIZE]; 60 | 61 | /// time got in the packet containing first angle 62 | road_time_t time; 63 | /// timerange = diff( t(angle=0) - t(lastangle) ) 64 | road_timerange_t timerange; 65 | 66 | // FIXME: rename 67 | /// not all polarData are useful, use range to know until which index you can use the data 68 | int16_t range; 69 | } VelodynePolarData; 70 | 71 | #pragma pack(pop) 72 | 73 | #endif // STRUCTURE_VELODYNE_H 74 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/entries: -------------------------------------------------------------------------------- 1 | 12 2 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/format: -------------------------------------------------------------------------------- 1 | 12 2 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/36/367630f4a6f38b7077d89e4e9b8ce507b164e3db.svn-base: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/58/58a1159d9132ad287eca14df70e47f43f7326b77.svn-base: -------------------------------------------------------------------------------- 1 | project(VelodynePCLViewer) 2 | 3 | set(${PROJECT_NAME}_VERSION_MAJOR 0) 4 | set(${PROJECT_NAME}_VERSION_MINOR 1) 5 | 6 | ################################################################################ 7 | # PACPUS 8 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME}) 9 | 10 | ################################################################################ 11 | # INCLUDE 12 | include_directories(${PROJECT_BINARY_DIR}) 13 | include_directories(${PROJECT_SOURCE_DIR}) 14 | include_directories(${CMAKE_SOURCE_DIR}/SensorComponent) 15 | link_directories(${PACPUSBIN_DIRECTORY}) 16 | 17 | ################################################################################ 18 | # SOURCES 19 | set(VelodynePCLViewer 20 | main.cpp 21 | ComputingComponent.cpp 22 | ) 23 | 24 | #SET_SOURCE_FILES_PROPERTIES(file.h 25 | # HEADER_FILE_ONLY TRUE 26 | #) 27 | 28 | set(VelodynePCLViewer_UI_SOURCES 29 | ui/widgetPCL.cpp 30 | ) 31 | source_group(UI FILES ${CLD_UI_SOURCES}) 32 | 33 | set(VelodynePCLViewer_MOC_CLASSES 34 | ComputingComponent.h 35 | ui/widgetPCL.h 36 | ) 37 | 38 | set(VelodynePCLViewer_UI_CLASSES 39 | ) 40 | 41 | set(XMLS 42 | XML/VelodynePCLViewer.xml 43 | XML/VelodynePCLViewer_d.xml 44 | ) 45 | 46 | #/////////////////////////////////////////////////////////////////////// 47 | # QT4 48 | if(QT4_FOUND) 49 | QT4_WRAP_UI(VelodynePCLViewer_UI_SOURCES_H ${VelodynePCLViewer_UI_CLASSES}) 50 | qt4_wrap_cpp(VelodynePCLViewer_MOC_SOURCES ${VelodynePCLViewer_MOC_CLASSES}) 51 | message(STATUS PLUGIN_H=${PLUGIN_H}) 52 | message(STATUS PLUGIN_CPP=${PLUGIN_CPP}) 53 | endif(QT4_FOUND) 54 | 55 | ################################################################################ 56 | # BUILD and LINK 57 | add_executable(${PROJECT_NAME} 58 | ${VelodynePCLViewer} 59 | ${VelodynePCLViewer_UI_SOURCES} 60 | ${VelodynePCLViewer_MOC_SOURCES} 61 | ${VelodynePCLViewer_UI_SOURCES_H} 62 | ) 63 | # LIBS 64 | list(APPEND LIBS 65 | SensorComponent 66 | PacpusTools PacpusLib FileLib 67 | ${PCL_LIBRARIES} QVTK vtkHybrid ${PCL_IO_LIBRARIES} pcl_visualization 68 | ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${GLEW_LIBRARY} 69 | ${QT_LIBRARIES} 70 | ) 71 | target_link_libraries(${PROJECT_NAME} ${LIBS}) 72 | # PROPERTIES 73 | set_target_properties(${PROJECT_NAME} PROPERTIES DEBUG_POSTFIX "_d") 74 | 75 | if(PACPUS_FOLDERS) 76 | set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "tools") 77 | endif() 78 | 79 | ################################################################################ 80 | # INSTALL 81 | install(TARGETS ${PROJECT_NAME} 82 | RUNTIME DESTINATION ${PACPUSBIN_DIRECTORY} 83 | LIBRARY DESTINATION ${PACPUSBIN_DIRECTORY} 84 | ARCHIVE DESTINATION ${PACPUSLIB_DIRECTORY} 85 | ) 86 | 87 | install(FILES ${XMLS} 88 | DESTINATION ${PACPUSBIN_DIRECTORY}/XML 89 | ) 90 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/7f/7f44c14ec58f418bfb2e8542f9033633faafbfac.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: Lidar Detection 4 | 5 | @date created 2010-06-03 16:13 6 | @author Julien Moras 7 | @author Sergio Rodriguez 8 | @version $Id: $ 9 | */ 10 | 11 | #include "ComputingComponent.h" 12 | 13 | #include 14 | #include 15 | #include 16 | //#include 17 | //#include 18 | //#include 19 | //#include 20 | //#include 21 | //#include 22 | #include 23 | //#include 24 | //#include 25 | #include 26 | //#include 27 | //#include 28 | //#include 29 | //#include 30 | //#include 31 | #include 32 | #include 33 | 34 | #include "kernel/ComponentFactory.h" 35 | #include "kernel/Log.h" 36 | #include "PacpusTools/ShMem.h" 37 | 38 | #include "ui/widgetPCL.h" 39 | 40 | using ::std::string; 41 | 42 | using namespace pacpus; 43 | 44 | DECLARE_STATIC_LOGGER("pacpus.cityvip.VelodynePCLViewer.ComputingComponent"); 45 | 46 | const char * ComputingComponent::COMPONENT_NAME = "ComputingComponent"; 47 | const char * ComputingComponent::COMPONENT_XML_NAME = "computingComponent"; 48 | 49 | /// Construct the factory 50 | static ComponentFactory sFactory(ComputingComponent::COMPONENT_NAME); 51 | 52 | ComputingComponent::ComputingComponent(QString name) 53 | : ComponentBase(name) 54 | { 55 | LOG_TRACE("constructor(" << name <<")"); 56 | } 57 | 58 | ComputingComponent::~ComputingComponent() 59 | { 60 | LOG_TRACE("destructor"); 61 | } 62 | 63 | ComponentBase::COMPONENT_CONFIGURATION ComputingComponent::configureComponent(XmlComponentConfig /*config*/) 64 | { 65 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 66 | 67 | //Load Xml parameters 68 | 69 | LOG_INFO("configured component ComputingComponent"); 70 | return ComponentBase::CONFIGURED_OK; 71 | } 72 | 73 | void ComputingComponent::startActivity() 74 | { 75 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 76 | 77 | startComponent(); 78 | } 79 | 80 | void ComputingComponent::stopActivity() 81 | { 82 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 83 | 84 | stopComponent(); 85 | } 86 | 87 | void ComputingComponent::startComponent() 88 | { 89 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 90 | 91 | // Positioning component searching 92 | ComponentManager * mgr = ComponentManager::getInstance(); 93 | 94 | m_VelodyneInterface = static_cast(mgr->getComponent("velodyneInterface")); 95 | m_VelodyneInterface->setVelodyneComputingStrategy(this); 96 | 97 | wi = new WidgetPCL(); 98 | 99 | LOG_INFO("started component '" << componentName << "'"); 100 | } 101 | 102 | void ComputingComponent::stopComponent() 103 | { 104 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 105 | 106 | delete wi; 107 | //PCL SAVE RESULTS 108 | 109 | string pcdAsciiFilePath = "test_pcd.pcd"; 110 | pcl::io::savePCDFileASCII(pcdAsciiFilePath, globale_cloud); 111 | LOG_INFO("saved cloud with " << globale_cloud.points.size() << " data points to file '" << pcdAsciiFilePath.c_str() << "'"); 112 | /* for (size_t i = 0; i < cloud2.points.size (); ++i) 113 | std::cerr << " " << cloud2.points[i].x << " " << cloud2.points[i].y << " " << cloud2.points[i].z << std::endl; 114 | */ 115 | 116 | LOG_INFO("stopped component '" << componentName << "'"); 117 | } 118 | 119 | void ComputingComponent::processRaw(VelodynePolarData * /*incomingData*/) 120 | { 121 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 122 | } 123 | 124 | void ComputingComponent::processCorrected(VelodyneCartData * incomingData) 125 | { 126 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 127 | //local run variables 128 | pcl::PointCloud::Ptr locale_cloud (new pcl::PointCloud); 129 | 130 | SetPointCloudFromScan(incomingData,locale_cloud); 131 | 132 | wi->updatePointCloud(locale_cloud->makeShared()); 133 | } 134 | 135 | void ComputingComponent::SetPointCloudFromScan(VelodyneCartData * m_incomingData,pcl::PointCloud::Ptr locale_cloud) 136 | { 137 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 138 | 139 | int width=VELODYNE_SCAN_SIZE; 140 | int height=32; 141 | 142 | // Fill in the cloud data 143 | locale_cloud->width = width; 144 | locale_cloud->height = height; 145 | locale_cloud->is_dense = false; 146 | locale_cloud->points.resize (locale_cloud->width * locale_cloud->height); 147 | 148 | for (int i = 0; i < width; ++i) 149 | { 150 | for (int j = 0; j < height ; ++j) 151 | { 152 | locale_cloud->points[i*height+j].x = m_incomingData->Data[i].Points[j].X; 153 | locale_cloud->points[i*height+j].y = m_incomingData->Data[i].Points[j].Y; 154 | locale_cloud->points[i*height+j].z = m_incomingData->Data[i].Points[j].Z; 155 | //qDebug() << "i" << i << "j" << j<< "index" <::Ptr locale_cloud) 161 | { 162 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 163 | 164 | locale_cloud->width = m_incomingData->nbPoint; 165 | locale_cloud->height = 1; 166 | locale_cloud->is_dense = false; 167 | locale_cloud->points.resize (locale_cloud->width * locale_cloud->height); 168 | 169 | for (size_t i = 0; i < locale_cloud->points.size (); ++i) 170 | { 171 | locale_cloud->points[i].x = m_incomingData->point[i].x/100.0; 172 | locale_cloud->points[i].y = m_incomingData->point[i].y/100.0; 173 | locale_cloud->points[i].z = m_incomingData->point[i].z/100.0; 174 | } 175 | } 176 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/84/84b80d7aa2724400f8602f142a597807c14c0495.svn-base: -------------------------------------------------------------------------------- 1 | #include "widgetPCL.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | //#include 11 | 12 | #include "kernel/Log.h" 13 | 14 | using namespace pacpus; 15 | 16 | const std::vector WidgetPCL::Default_Color(3, 255); 17 | 18 | DECLARE_STATIC_LOGGER("pacpus.cityvip.WidgetPCL"); 19 | 20 | WidgetPCL::WidgetPCL() 21 | { 22 | LOG_TRACE("constructor"); 23 | 24 | // renderWindow=NULL; 25 | 26 | /* 27 | firstTime =true; 28 | counter=true; 29 | received_new_cloud_data=false; 30 | */ 31 | 32 | start(); 33 | } 34 | 35 | WidgetPCL::~WidgetPCL() 36 | { 37 | LOG_TRACE("destructor"); 38 | 39 | delete viewer; 40 | } 41 | 42 | /* 43 | vtkSmartPointer WidgetPCL::getRenderWindows() { 44 | return renderWindow; 45 | } 46 | */ 47 | 48 | void WidgetPCL::run() 49 | { 50 | // FIXME: deleted in destructor, move instantiation to constructor, otherwise memory leak 51 | viewer = new pcl::visualization::PCLVisualizer("3D Viewer"); 52 | 53 | viewer->setBackgroundColor (0, 0, 0); 54 | //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 55 | viewer->addCoordinateSystem (1.0); 56 | viewer->initCameraParameters (); 57 | 58 | /* 59 | renderWindow = viewer->getRenderWindow(); 60 | 61 | widget = new QVTKWidget(); 62 | widget->resize(512, 256); 63 | widget->SetRenderWindow(renderWindow); 64 | widget->setWindowTitle("ok"); 65 | widget->show(); 66 | */ 67 | 68 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 69 | cloud_xyz=cloud; 70 | 71 | while (!viewer->wasStopped()) { 72 | // sleep(1); 73 | /* 74 | if (received_new_cloud_data) { 75 | QMutexLocker mutexLocker(&mutex); 76 | Q_UNUSED(mutexLocker); 77 | 78 | received_new_cloud_data = false; 79 | 80 | viewer->removePointCloud("1"); 81 | viewer->addPointCloud(cloud_xyz,"1"); 82 | } 83 | */ 84 | 85 | while(!cloudPoint_queue.empty()) { 86 | QMutexLocker mutexLocker(&mutex); 87 | Q_UNUSED(mutexLocker); 88 | 89 | //received_new_cloud_data = false; 90 | 91 | viewer->removePointCloud(cloudPoint_queue.front().name); 92 | viewer->addPointCloud(cloudPoint_queue.front().cloud_xyz,cloudPoint_queue.front().color,cloudPoint_queue.front().name,cloudPoint_queue.front().viewport); 93 | 94 | //std::cerr << "size" << cloudPoint_queue.size() <<"Name " << cloudPoint_queue.front().name << "cloud size " << std::endl; 95 | cloudPoint_queue.pop(); 96 | } 97 | 98 | viewer->spinOnce(100); 99 | // qDebug() << "run finish" << cloud_xyz->width; 100 | } 101 | } 102 | 103 | /* 104 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz) 105 | { 106 | mutex.lock(); 107 | cloud_xyz=new_cloud_xyz; 108 | mutex.unlock(); 109 | received_new_cloud_data=true; 110 | qDebug() << "Updated" ; 111 | } 112 | 113 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, QString new_name, int viewport) 114 | { 115 | pcl::visualization::PointCloudColorHandlerCustom new_color = pcl::visualization::PointCloudColorHandlerCustom(new_cloud_xyz,255,255,255); 116 | updatePointCloud(new_cloud_xyz,new_color,new_name,viewport); 117 | } 118 | 119 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, pcl::visualization::PointCloudColorHandlerCustom new_color, QString new_name , int viewport) 120 | { 121 | CloudPointXYZDisplay CloudPoint ={new_cloud_xyz,new_color,new_name.toStdString(),viewport}; 122 | mutex.lock(); 123 | cloud_queue.push(new_cloud_xyz); 124 | cloud_name_queue.push(new_name); 125 | cloudPoint_queue.push(CloudPoint); 126 | mutex.unlock(); 127 | qDebug() << "Updated" ; 128 | } 129 | */ 130 | 131 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, QString new_name, std::vector new_color_val, int viewport) 132 | { 133 | LOG_TRACE("updating point cloud..."); 134 | 135 | pcl::visualization::PointCloudColorHandlerCustom new_color = 136 | pcl::visualization::PointCloudColorHandlerCustom(new_cloud_xyz, new_color_val[0], new_color_val[1], new_color_val[2]); 137 | CloudPointXYZDisplay CloudPoint = {new_cloud_xyz, new_color, new_name.toStdString(), viewport}; 138 | 139 | QMutexLocker mutexLocker(&mutex); 140 | Q_UNUSED(mutexLocker); 141 | 142 | /*cloud_queue.push(new_cloud_xyz); 143 | cloud_name_queue.push(new_name);*/ 144 | cloudPoint_queue.push(CloudPoint); 145 | 146 | LOG_DEBUG("updated point cloud"); 147 | } 148 | 149 | /* 150 | void WidgetPCL::UpdatePointCloud(pcl::PointCloud::ConstPtr cloud) { 151 | 152 | if (!viewer->addPointCloud (cloud, "sample cloud")) 153 | viewer->addPointCloud (cloud, "sample cloud"); 154 | viewer->getRenderWindow(); 155 | } 156 | */ 157 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/8c/8cb5e7b83ff392a3e5162338371b361605e65bcc.svn-base: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/ac/ac3ad1c9c4284a4c3969468c1b7d5660d4c42b4b.svn-base: -------------------------------------------------------------------------------- 1 | #ifndef STRUCTURE_VELODYNE_CART_H 2 | #define STRUCTURE_VELODYNE_CART_H 3 | 4 | #define VELODYNE_BLOCK_SIZE 100 5 | #define VELODYNE_PACKET_SIZE 1206 6 | #define VELODYNE_SCAN_SIZE 4166 7 | #define VELODYNE_NB_BLOCKS_PER_PACKET 12 8 | 9 | #pragma pack(push, 1) 10 | 11 | // 25 bytes size 12 | typedef struct VelodyneCartPoint 13 | { 14 | float distance; // in meter - if 0 no return up to 65m 15 | double X,Y,Z; // in meter if 0 no return up to 65m 16 | unsigned char intensity; // 255 most intense return 17 | }VelodyneCartPoint; 18 | 19 | // 100=VELODYNE_BLOCK_SIZE bytes size 20 | typedef struct VelodyneCartBlock 21 | { 22 | unsigned short block; // 0xEEFF upper and 0xDDFF lower 23 | float alpha; // azimuth in degrees 24 | float beta; // elevation in degrees 25 | struct VelodyneCartPoint Points[32]; // a couple of distance and intensity representing a point 26 | }VelodyneCartBlock; 27 | 28 | 29 | // size : 100*4166 + 2*4166 + 8 + 4 = 424 944 bytes 30 | // structure containing data of a complete revolution of the lidar 31 | typedef struct VelodyneCartData 32 | { 33 | VelodyneCartBlock Data[VELODYNE_SCAN_SIZE]; // data of one block of 32 beams (upper or lower) 34 | unsigned short scanCount[VELODYNE_SCAN_SIZE]; // scan count relative to polarData above 35 | road_time_t time; // time got in the packet containing first angle 36 | road_timerange_t timerange; // timerange = diff( t(angle=0) - t(lastangle) ) 37 | short range; // not all polarData are useful, use range to know until which index you can use the data 38 | }VelodyneCartData; 39 | 40 | #pragma pack(pop) 41 | 42 | #endif // STRUCTURE_VELODYNE_CART_H 43 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/bb/bbca14642c5caa2d6601aebd1e5688737cde6f83.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: CLD_Demo 4 | 5 | @date created 2009-11-03 16:13 6 | @version $Id: $ 7 | @author Julien Moras 8 | */ 9 | 10 | #include 11 | #ifndef WIN32 12 | # include 13 | #endif // WIN32 14 | #include 15 | 16 | #include "kernel/ComponentFactory.h" 17 | #include "kernel/ComponentManager.h" 18 | #include "kernel/Log.h" 19 | #include "ComputingComponent.h" 20 | 21 | using namespace pacpus; 22 | 23 | // ================================================================================ 24 | using ::boost::shared_ptr; 25 | 26 | // ================================================================================ 27 | DECLARE_STATIC_LOGGER("pacpus.cityvip.VelodynePCLViewer"); 28 | 29 | #ifdef NDEBUG 30 | static const QString kDefaultXmlConfigFilePath = "XML/VelodynePCLViewer.xml"; 31 | #else 32 | static const QString kDefaultXmlConfigFilePath = "XML/VelodynePCLViewer_d.xml"; 33 | #endif 34 | 35 | // ================================================================================ 36 | int main(int argc, char * argv[]) 37 | { 38 | #ifndef WIN32 39 | // configure logging facility 40 | ::log4cxx::BasicConfigurator::configure(); 41 | # ifdef NDEBUG 42 | ::log4cxx::Logger::getRootLogger()->setLevel(log4cxx::Level::getInfo()); 43 | # else 44 | ::log4cxx::Logger::getRootLogger()->setLevel(log4cxx::Level::getDebug()); 45 | # endif 46 | #endif // WIN32 47 | 48 | // ================================================================================ 49 | QApplication app(argc, argv); 50 | shared_ptr mgr(ComponentManager::getInstance(), ComponentManager::destroyer()); 51 | 52 | QString configFilePath; 53 | if (argc > 1) { 54 | configFilePath = argv[1]; 55 | } else { 56 | LOG_WARN("no XML config file specified. Using default: " << kDefaultXmlConfigFilePath); 57 | configFilePath = kDefaultXmlConfigFilePath; 58 | } 59 | LOG_INFO("loading file '" << configFilePath << "'"); 60 | mgr->loadComponents(configFilePath); 61 | 62 | // start components 63 | mgr->start("computingComponent"); 64 | mgr->start("velodyneInterface"); 65 | 66 | // save application exit status 67 | int exitStatus = app.exec(); 68 | 69 | // stop all components before exiting app 70 | mgr->stop(); 71 | 72 | // return application exit status 73 | return exitStatus; 74 | } 75 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/d3/d32074f3a4a5ab6de4d8491a3cf2bdf182fdceed.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: Lidar Detection 4 | 5 | @date created 2010-06-03 16:13 6 | @author Julien Moras 7 | @author Sergio Rodriguez 8 | @version $Id: $ 9 | */ 10 | 11 | #ifndef ComputingComponent_H 12 | #define ComputingComponent_H 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "kernel/ComponentBase.h" 22 | #include "structure/structureCanCarmen.h" 23 | #include "structure/structure_telemetre.h" // ScanAlascaData 24 | #include "structure/structure_IGN.h" // Pose2Denu 25 | 26 | #include "VelodyneInterface.h" 27 | #include "LidarInterface.h" 28 | 29 | #include "../CLDLib/CityVIP_common.h" 30 | 31 | class QImage; 32 | class QTimer; 33 | 34 | namespace pacpus { 35 | 36 | class ShMem; 37 | class WidgetPCL; 38 | 39 | class ComputingComponent : public QObject, public ComponentBase, public VelodyneComputingStrategy 40 | { 41 | Q_OBJECT 42 | 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | static const char * COMPONENT_NAME; 47 | static const char * COMPONENT_XML_NAME; 48 | 49 | ComputingComponent(QString name); 50 | ~ComputingComponent(); 51 | 52 | virtual void stopActivity(); 53 | virtual void startActivity(); 54 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 55 | 56 | void startComponent(); 57 | void stopComponent(); 58 | 59 | void processRaw(VelodynePolarData *); 60 | void processCorrected(VelodyneCartData *); 61 | 62 | private: 63 | void SetPointCloudFromScan(VelodyneCartData *, pcl::PointCloud::Ptr); 64 | void SetPointCloudFromScan(ScanAlascaData *, pcl::PointCloud::Ptr); 65 | 66 | QMutex m_mutex; 67 | 68 | int m_fps; 69 | 70 | pcl::PointCloud globale_cloud; 71 | VelodyneInterface * m_VelodyneInterface; 72 | WidgetPCL * wi; 73 | }; 74 | 75 | } 76 | 77 | #endif // ComputingComponent_H 78 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/pristine/f3/f3594cc2a482dcae92be813ce592349a5e4b1d03.svn-base: -------------------------------------------------------------------------------- 1 | #ifndef WIDGETPCL_H 2 | #define WIDGETPCL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | //#include "vtkSmartPointer.h" 14 | //#include "vtkRenderWindow.h" 15 | 16 | //class QVTKWidget; 17 | 18 | namespace pcl { namespace visualization { 19 | class PCLVisualizer; 20 | } } 21 | 22 | struct CloudPointXYZDisplay 23 | { 24 | pcl::PointCloud::Ptr cloud_xyz; 25 | pcl::visualization::PointCloudColorHandlerCustom color; 26 | std::string name; 27 | int viewport; 28 | }; 29 | 30 | namespace pacpus { 31 | 32 | class WidgetPCL 33 | : public QThread 34 | { 35 | Q_OBJECT 36 | 37 | public: 38 | WidgetPCL(); 39 | ~WidgetPCL(); 40 | 41 | static const std::vector Default_Color; 42 | 43 | //vtkSmartPointer getRenderWindows(); 44 | //void updatePointCloud(pcl::PointCloud::Ptr); 45 | //void updatePointCloud(pcl::PointCloud::Ptr, QString = "cloud", int = 0); 46 | //void updatePointCloud(pcl::PointCloud::Ptr, pcl::visualization::PointCloudColorHandlerCustom ,QString = "cloud", int = 0); 47 | void updatePointCloud(pcl::PointCloud::Ptr, QString = "cloud", std::vector = Default_Color, int = 0); 48 | //QVTKWidget * widget; 49 | 50 | protected: 51 | void run(); 52 | 53 | private: 54 | std::queue cloudPoint_queue; 55 | pcl::PointCloud::Ptr cloud_xyz; 56 | std::queue::Ptr> cloud_queue; 57 | pcl::visualization::PCLVisualizer * viewer; 58 | QString name; 59 | std::queue cloud_name_queue; 60 | 61 | //vtkSmartPointer renderWindow; 62 | //bool firstTime,counter,received_new_cloud_data; 63 | QMutex mutex; 64 | }; 65 | 66 | } 67 | 68 | #endif // WIDGETPCL_H 69 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/.svn/wc.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mdamien/velodyne/301033244859c52f75a380f45e30df365324afa2/pacpussensors/tx_p12/.svn/wc.db -------------------------------------------------------------------------------- /pacpussensors/tx_p12/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(VelodynePCLViewer) 2 | 3 | ################################################################################ 4 | add_definitions( -DVelodynePCLViewer_EXPORTS ) 5 | 6 | # ======================================== 7 | # Configure qt4 8 | # ======================================== 9 | if(QT4_FOUND) 10 | set(QT_USE_QTXML true) 11 | set(QT_USE_QTNETWORK true) 12 | include(${QT_USE_FILE}) 13 | else() 14 | message(ERROR "Qt4 needed") 15 | endif() 16 | 17 | # PCL 18 | find_package(PCL 1.2 REQUIRED) 19 | 20 | include_directories(${PCL_INCLUDE_DIRS}) 21 | link_directories(${PCL_LIBRARY_DIRS}) 22 | add_definitions(${PCL_DEFINITIONS}) 23 | 24 | # ======================================== 25 | # Compiler definitions 26 | # ======================================== 27 | add_definitions( 28 | ${QT_DEFINITIONS} 29 | ) 30 | 31 | # ======================================== 32 | # Include directories 33 | # ======================================== 34 | include_directories( 35 | ${PROJECT_BINARY_DIR} 36 | ${QT_INCLUDE_DIR} 37 | ) 38 | 39 | # ======================================== 40 | # Link directories 41 | # ======================================== 42 | link_directories( ${PACPUS_LIB_DIR} 43 | ) 44 | 45 | pacpus_plugin(PLUGIN_CPP PLUGIN_H ${PROJECT_NAME} ) 46 | 47 | set(HDRS 48 | 49 | ) 50 | 51 | 52 | # ======================================== 53 | # List of sources 54 | # ======================================== 55 | set( 56 | PROJECT_SRCS 57 | ComputingComponent.cpp 58 | ui/widgetPCL.cpp 59 | VelodyneInterface.cpp 60 | ${HDRS} 61 | ${PLUGIN_CPP} 62 | ) 63 | 64 | # ======================================== 65 | # Files to MOC 66 | # ======================================== 67 | set( 68 | FILES_TO_MOC 69 | ComputingComponent.h 70 | ui/widgetPCL.h 71 | VelodyneInterface.h 72 | ${PLUGIN_H} 73 | ) 74 | 75 | 76 | set( 77 | UI_FILES 78 | 79 | ) 80 | 81 | # ======================================== 82 | # Call MOC 83 | # ======================================== 84 | qt4_wrap_cpp( 85 | PROJECT_MOC_SRCS 86 | ${FILES_TO_MOC} 87 | ) 88 | 89 | qt4_wrap_ui( 90 | PROJECT_UI_SRCS 91 | ${UI_FILES} 92 | ) 93 | 94 | # ======================================== 95 | # Build a library 96 | # ======================================== 97 | pacpus_add_library( 98 | ${PROJECT_NAME} SHARED 99 | ${PROJECT_SRCS} 100 | ${PROJECT_MOC_SRCS} 101 | ${PROJECT_UI_SRCS} 102 | ) 103 | 104 | set(LIBS 105 | optimized FileLib debug FileLib_d 106 | optimized PacpusLib debug PacpusLib_d 107 | optimized PacpusTools debug PacpusTools_d 108 | ) 109 | if (WIN32) 110 | list(APPEND LIBS 111 | optimized ROAD_TIME debug ROAD_TIME_d 112 | ) 113 | endif() 114 | 115 | # ======================================== 116 | # Libraries 117 | # ======================================== 118 | # All the platform 119 | target_link_libraries( 120 | ${PROJECT_NAME} 121 | ${PACPUS_LIBRARIES} 122 | ${QT_LIBRARIES} 123 | ${PACPUS_DEPENDENCIES_LIB} 124 | ${LIBS} 125 | ${PCL_LIBRARIES} 126 | ) 127 | 128 | pacpus_folder(${PROJECT_NAME} "components") 129 | 130 | # ======================================== 131 | # Install 132 | # ======================================== 133 | pacpus_install(${PROJECT_NAME}) 134 | 135 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/ComputingComponent.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: Lidar Detection 4 | 5 | @date created 2010-06-03 16:13 6 | @author Julien Moras 7 | @author Sergio Rodriguez 8 | @version $Id: $ 9 | */ 10 | 11 | #include "ComputingComponent.h" 12 | 13 | #include 14 | #include 15 | #include 16 | //#include 17 | //#include 18 | //#include 19 | //#include 20 | //#include 21 | //#include 22 | #include 23 | //#include 24 | //#include 25 | #include 26 | //#include 27 | //#include 28 | //#include 29 | //#include 30 | //#include 31 | #include 32 | #include 33 | 34 | #include "kernel/ComponentFactory.h" 35 | #include "kernel/Log.h" 36 | #include "PacpusTools/ShMem.h" 37 | 38 | #include "ui/widgetPCL.h" 39 | 40 | using ::std::string; 41 | 42 | using namespace pacpus; 43 | 44 | DECLARE_STATIC_LOGGER("pacpus.cityvip.VelodynePCLViewer.ComputingComponent"); 45 | 46 | const char * ComputingComponent::COMPONENT_NAME = "ComputingComponent"; 47 | const char * ComputingComponent::COMPONENT_XML_NAME = "computingComponent"; 48 | 49 | /// Construct the factory 50 | static ComponentFactory sFactory(ComputingComponent::COMPONENT_NAME); 51 | 52 | ComputingComponent::ComputingComponent(QString name) 53 | : ComponentBase(name) 54 | { 55 | LOG_TRACE("constructor(" << name <<")"); 56 | } 57 | 58 | ComputingComponent::~ComputingComponent() 59 | { 60 | LOG_TRACE("destructor"); 61 | } 62 | 63 | ComponentBase::COMPONENT_CONFIGURATION ComputingComponent::configureComponent(XmlComponentConfig /*config*/) 64 | { 65 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 66 | 67 | //Load Xml parameters 68 | 69 | LOG_INFO("configured component ComputingComponent"); 70 | return ComponentBase::CONFIGURED_OK; 71 | } 72 | 73 | void ComputingComponent::startActivity() 74 | { 75 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 76 | 77 | startComponent(); 78 | } 79 | 80 | void ComputingComponent::stopActivity() 81 | { 82 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 83 | 84 | stopComponent(); 85 | } 86 | 87 | void ComputingComponent::startComponent() 88 | { 89 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 90 | 91 | // Positioning component searching 92 | ComponentManager * mgr = ComponentManager::getInstance(); 93 | 94 | m_VelodyneInterface = static_cast(mgr->getComponent("velodyneInterface")); 95 | m_VelodyneInterface->setVelodyneComputingStrategy(this); 96 | 97 | wi = new WidgetPCL(); 98 | 99 | LOG_INFO("started component '" << componentName << "'"); 100 | } 101 | 102 | void ComputingComponent::stopComponent() 103 | { 104 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 105 | 106 | delete wi; 107 | //PCL SAVE RESULTS 108 | 109 | string pcdAsciiFilePath = "test_pcd.pcd"; 110 | pcl::io::savePCDFileASCII(pcdAsciiFilePath, globale_cloud); 111 | LOG_INFO("saved cloud with " << globale_cloud.points.size() << " data points to file '" << pcdAsciiFilePath.c_str() << "'"); 112 | /* for (size_t i = 0; i < cloud2.points.size (); ++i) 113 | std::cerr << " " << cloud2.points[i].x << " " << cloud2.points[i].y << " " << cloud2.points[i].z << std::endl; 114 | */ 115 | 116 | LOG_INFO("stopped component '" << componentName << "'"); 117 | } 118 | 119 | void ComputingComponent::processRaw(VelodynePolarData * /*incomingData*/) 120 | { 121 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 122 | } 123 | 124 | void ComputingComponent::processCorrected(VelodyneCartData * incomingData) 125 | { 126 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 127 | //local run variables 128 | pcl::PointCloud::Ptr locale_cloud (new pcl::PointCloud); 129 | 130 | SetPointCloudFromScan(incomingData,locale_cloud); 131 | 132 | wi->updatePointCloud(locale_cloud->makeShared()); 133 | } 134 | 135 | void ComputingComponent::SetPointCloudFromScan(VelodyneCartData * m_incomingData,pcl::PointCloud::Ptr locale_cloud) 136 | { 137 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 138 | 139 | int width=VELODYNE_SCAN_SIZE; 140 | int height=32; 141 | 142 | // Fill in the cloud data 143 | locale_cloud->width = width; 144 | locale_cloud->height = height; 145 | locale_cloud->is_dense = false; 146 | locale_cloud->points.resize (locale_cloud->width * locale_cloud->height); 147 | 148 | for (int i = 0; i < width; ++i) 149 | { 150 | for (int j = 0; j < height ; ++j) 151 | { 152 | locale_cloud->points[i*height+j].x = m_incomingData->Data[i].Points[j].X; 153 | locale_cloud->points[i*height+j].y = m_incomingData->Data[i].Points[j].Y; 154 | locale_cloud->points[i*height+j].z = m_incomingData->Data[i].Points[j].Z; 155 | //qDebug() << "i" << i << "j" << j<< "index" <::Ptr locale_cloud) 162 | { 163 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 164 | 165 | locale_cloud->width = m_incomingData->nbPoint; 166 | locale_cloud->height = 1; 167 | locale_cloud->is_dense = false; 168 | locale_cloud->points.resize (locale_cloud->width * locale_cloud->height); 169 | 170 | for (size_t i = 0; i < locale_cloud->points.size (); ++i) 171 | { 172 | locale_cloud->points[i].x = m_incomingData->point[i].x/100.0; 173 | locale_cloud->points[i].y = m_incomingData->point[i].y/100.0; 174 | locale_cloud->points[i].z = m_incomingData->point[i].z/100.0; 175 | } 176 | } 177 | */ 178 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/ComputingComponent.h: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: Lidar Detection 4 | 5 | @date created 2010-06-03 16:13 6 | @author Julien Moras 7 | @author Sergio Rodriguez 8 | @version $Id: $ 9 | */ 10 | 11 | #ifndef ComputingComponent_H 12 | #define ComputingComponent_H 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "kernel/ComponentBase.h" 22 | //#include "structure/structureCanCarmen.h" 23 | //#include "structure/structure_telemetre.h" // ScanAlascaData 24 | //#include "structure/structure_IGN.h" // Pose2Denu 25 | 26 | #include "VelodyneInterface.h" 27 | //#include "LidarInterface.h" 28 | 29 | //#include "../CLDLib/CityVIP_common.h" 30 | 31 | class QImage; 32 | class QTimer; 33 | 34 | namespace pacpus { 35 | 36 | class ShMem; 37 | class WidgetPCL; 38 | 39 | class ComputingComponent : public QObject, public ComponentBase, public VelodyneComputingStrategy 40 | { 41 | Q_OBJECT 42 | 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | static const char * COMPONENT_NAME; 47 | static const char * COMPONENT_XML_NAME; 48 | 49 | ComputingComponent(QString name); 50 | ~ComputingComponent(); 51 | 52 | virtual void stopActivity(); 53 | virtual void startActivity(); 54 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 55 | 56 | void startComponent(); 57 | void stopComponent(); 58 | 59 | void processRaw(VelodynePolarData *); 60 | void processCorrected(VelodyneCartData *); 61 | 62 | private: 63 | void SetPointCloudFromScan(VelodyneCartData *, pcl::PointCloud::Ptr); 64 | // void SetPointCloudFromScan(ScanAlascaData *, pcl::PointCloud::Ptr); 65 | 66 | QMutex m_mutex; 67 | 68 | int m_fps; 69 | 70 | pcl::PointCloud globale_cloud; 71 | VelodyneInterface * m_VelodyneInterface; 72 | WidgetPCL * wi; 73 | }; 74 | 75 | } 76 | 77 | #endif // ComputingComponent_H 78 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/VelodyneInterface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | 4 | @date created 2010-09-03 5 | @author Julien Moras 6 | */ 7 | 8 | #include "VelodyneInterface.h" 9 | 10 | #include "kernel/ComponentFactory.h" 11 | #include "kernel/Log.h" 12 | #include "PacpusTools/geodesie.h" 13 | #include "PacpusTools/ShMem.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace pacpus { 22 | 23 | using namespace std; 24 | 25 | DECLARE_STATIC_LOGGER("pacpus.cityvip.VelodyneInterface"); 26 | 27 | const char * VelodyneInterface::COMPONENT_NAME = "VelodyneInterface"; 28 | const char * VelodyneInterface::COMPONENT_XML_NAME = "VelodyneInterface"; 29 | const char * VelodyneInterface::SHARED_MEMORY_NAME = "VELODYNE"; 30 | 31 | const unsigned kMaxWaitForThreadTimeMs = 5000; 32 | 33 | /// Construct the factory 34 | static ComponentFactory sFactory(VelodyneInterface::COMPONENT_NAME); 35 | 36 | VelodyneInterface::VelodyneInterface(QString name) 37 | : ComponentBase(name) 38 | { 39 | LOG_TRACE("constructor(" << name <<")"); 40 | } 41 | 42 | VelodyneInterface::~VelodyneInterface() 43 | { 44 | LOG_TRACE("destructor"); 45 | } 46 | 47 | ComponentBase::COMPONENT_CONFIGURATION 48 | VelodyneInterface::configureComponent(XmlComponentConfig /* config */) 49 | { 50 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 51 | 52 | m_isThreadAlive = false; 53 | //Load Xml parameters 54 | //recording_ = (param.getProperty("output_file") == "true" ? true : false); 55 | //dbt2txt_ = (param.getProperty("output_dbt2txt") == "true" ? true : false); 56 | 57 | return ComponentBase::CONFIGURED_OK; 58 | } 59 | 60 | void VelodyneInterface::startActivity() 61 | { 62 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 63 | 64 | LOG_DEBUG("creating shared memory for Velodyne, size = " << sizeof(VelodynePolarData)); 65 | // initialize shared memory 66 | shmem_ = new ShMem(VelodyneInterface::SHARED_MEMORY_NAME, sizeof(VelodynePolarData)); 67 | 68 | // set thread state to alive 69 | VelodyneInterface::m_isThreadAlive = true; 70 | 71 | // start thread 72 | QThread::start(); 73 | 74 | LOG_INFO("started component '" << componentName << "'"); 75 | } 76 | 77 | void VelodyneInterface::stopActivity() 78 | { 79 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 80 | 81 | //Stop thread and Release variables 82 | VelodyneInterface::m_isThreadAlive = false; 83 | 84 | if (!QThread::wait(kMaxWaitForThreadTimeMs)) { 85 | QThread::terminate(); 86 | LOG_ERROR("component thread '" << componentName << "' was blocking" 87 | << ". It has been terminated" 88 | ); 89 | } 90 | delete shmem_; shmem_ = NULL; 91 | } 92 | 93 | void VelodyneInterface::run() 94 | { 95 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 96 | 97 | { 98 | // load lidar corrections 99 | string correctionsFilePath = "db.xml"; 100 | LOG_INFO("loading Velodyne corrections file '" << correctionsFilePath.c_str() << "'"); 101 | loadCorrections(correctionsFilePath); 102 | } 103 | 104 | //local run variables 105 | void * ptr; // shmem pointer for reading 106 | VelodynePolarData * scan; 107 | // double seuil = 0.001; 108 | double dxy, X, Y, Z; 109 | 110 | while (VelodyneInterface::m_isThreadAlive) { // Variable activated by ComponentBase 111 | if (shmem_->wait()) { 112 | ptr = shmem_->read(); 113 | memcpy(&velodyneData_, ptr, sizeof(velodyneData_)); 114 | 115 | if (NULL != velodyneComputingStrategy) { 116 | velodyneComputingStrategy->processRaw(&velodyneData_); 117 | } 118 | scan = &velodyneData_; 119 | 120 | int pointCountTotal = 0; 121 | //velodyneCartData_.scanCount=scan->scanCount; 122 | if (VELODYNE_SCAN_SIZE < scan->range) { 123 | LOG_WARN("scan size (" << scan->range << ") greater than maximal allowed size (" << VELODYNE_SCAN_SIZE << ")"); 124 | } 125 | velodyneCartData_.range = scan->range; 126 | velodyneCartData_.time=scan->time; 127 | velodyneCartData_.timerange=scan->timerange; 128 | 129 | for (int block=0; block < scan->range /*VELODYNE_SCAN_SIZE*/; ++block) { 130 | const double kFieldOfViewRadians = Geodesie::Deg2Rad(26.8 / 64.0); 131 | float alphaRadians = Geodesie::Deg2Rad((scan->polarData[block].angle) / 100.0); 132 | float betaRadians = 0.0; 133 | int k = 0; 134 | 135 | unsigned blockSignature = scan->polarData[block].block; 136 | switch (blockSignature) { 137 | case kVelodyneUpperBlock: 138 | betaRadians = Geodesie::Deg2Rad(2) - kFieldOfViewRadians * 32; 139 | k=0; 140 | break; 141 | case kVelodyneLowerBlock: 142 | betaRadians = Geodesie::Deg2Rad(2) - kFieldOfViewRadians * 64; 143 | k=32; 144 | break; 145 | default: 146 | LOG_WARN("invalid signature in block " << block << ", signature = " << blockSignature); 147 | continue; 148 | } 149 | 150 | // qDebug() << "Velodyne : Cart :" << block << scan->range << alpha << beta; 151 | velodyneCartData_.Data[block].alpha=alphaRadians; 152 | velodyneCartData_.Data[block].beta=betaRadians; 153 | velodyneCartData_.Data[block].block=scan->polarData[block].block; 154 | 155 | for (int iPoint = 0; iPoint < kVelodynePointsPerBlock; ++iPoint) { 156 | if (0 == scan->polarData[block].rawPoints[iPoint].distance) { 157 | // point at scanner 158 | continue; 159 | } 160 | 161 | float d = (scan->polarData[block].rawPoints[iPoint].distance) / 500.0 + m_distCor[iPoint+k] / 100.0; // increments de 2mm => /500 pour avoir des m + correction (en cm) 162 | /* 163 | if (d < m_seuil) { 164 | break; 165 | } 166 | */ 167 | 168 | // Application des corrections du LIDAR (cf. doc velodyne) 169 | double cosVertAngle = cos(betaRadians); 170 | double sinVertAngle = sin(betaRadians); 171 | 172 | /* if ( -1.8< d * sinVertAngle + m_vOffsetCor[iPoint+k] / 100.0 * cosVertAngle) 173 | continue;*/ 174 | 175 | double cosRotAngle = cos(alphaRadians - Geodesie::Deg2Rad(m_rotCor[iPoint+k])); 176 | double sinRotAngle = sin(alphaRadians - Geodesie::Deg2Rad(m_rotCor[iPoint+k])); 177 | 178 | double hOffsetCorr = m_hOffsetCor[iPoint+k] / 100.0; 179 | double vOffsetCorr = m_vOffsetCor[iPoint+k] / 100.0; 180 | 181 | dxy = d * cosVertAngle - vOffsetCorr * sinVertAngle; 182 | X = dxy * sinRotAngle - hOffsetCorr * cosRotAngle; // x 183 | Y = dxy * cosRotAngle + hOffsetCorr * sinRotAngle; // y 184 | Z = d * sinVertAngle + vOffsetCorr * cosVertAngle; // z 185 | 186 | // on ajoute le point au vecteur qui sera transmis la grille 187 | // data.push_back(pt); 188 | velodyneCartData_.Data[block].Points[iPoint].distance=d; 189 | velodyneCartData_.Data[block].Points[iPoint].X=X; 190 | velodyneCartData_.Data[block].Points[iPoint].Y=Y; 191 | velodyneCartData_.Data[block].Points[iPoint].Z=Z; 192 | velodyneCartData_.Data[block].Points[iPoint].intensity=scan->polarData[block].rawPoints[iPoint].intensity; 193 | 194 | ++pointCountTotal; 195 | // qDebug() << "Velodyne : Cart :" << X << Y << Z << d << alpha << beta << scan->polarData[block].rawPoints[iPoint].intensity<< "N" << n; 196 | 197 | // on met a jour beta 198 | betaRadians += kFieldOfViewRadians; // 26.8 FOV vertical 199 | } 200 | } 201 | LOG_DEBUG("Velodyne : Cart :" << "point count total = " << pointCountTotal); 202 | if (NULL != velodyneComputingStrategy) { 203 | velodyneComputingStrategy->processCorrected(&velodyneCartData_); 204 | } 205 | } else { 206 | LOG_ERROR("lidar timeout"); 207 | } 208 | } 209 | LOG_INFO("ended thread execution"); 210 | } 211 | 212 | void VelodyneInterface::loadCorrections(const std::string & filename) 213 | { 214 | LOG_TRACE(BOOST_CURRENT_FUNCTION); 215 | 216 | ///////////////////////////////////////// 217 | // Read the DOM tree form file 218 | QFile f(filename.c_str()); 219 | if (!f.open(QIODevice::ReadOnly)) { 220 | LOG_ERROR("cannot open file '" << filename.c_str() << "'"); 221 | return; 222 | } 223 | QDomDocument doc; 224 | if (!doc.setContent(&f)) { 225 | LOG_ERROR("cannot read XML content of '" << filename.c_str() << "'"); 226 | f.close(); 227 | return; 228 | } 229 | f.close(); 230 | 231 | ///////////////////////////////////////// 232 | // Parse the DOM tree 233 | 234 | // The root node is supposed to be a "map" tag 235 | QDomElement root=doc.documentElement(); 236 | root=root.firstChild().toElement(); 237 | 238 | // Traverse its children 239 | QDomElement child=root.firstChild().toElement(); 240 | while (!child.isNull()) { 241 | if (child.tagName() == "points_") { 242 | QDomElement item=child.firstChild().toElement(); 243 | while (!item.isNull()) { 244 | if (item.tagName() == "item") { 245 | QDomElement px=item.firstChild().toElement(); 246 | while (!px.isNull()) { 247 | if (px.tagName() == "px") { 248 | int i = 0; 249 | QDomElement cor=px.firstChild().toElement(); 250 | while (!cor.isNull()) { 251 | if (cor.tagName() == "id_") { 252 | i = cor.text().toInt(); 253 | } 254 | if (cor.tagName() == "rotCorrection_") 255 | m_rotCor[i] = cor.text().toDouble(); 256 | 257 | if (cor.tagName() == "vertCorrection_") { 258 | m_vertCor[i] = cor.text().toDouble(); 259 | } 260 | if (cor.tagName() == "distCorrection_") { 261 | m_distCor[i] = cor.text().toDouble(); 262 | } 263 | if (cor.tagName() == "horizOffsetCorrection_") { 264 | m_hOffsetCor[i] = cor.text().toDouble(); 265 | } 266 | if (cor.tagName() == "vertOffsetCorrection_") { 267 | m_vOffsetCor[i] = cor.text().toDouble(); 268 | } 269 | cor = cor.nextSibling().toElement(); 270 | } 271 | } 272 | px = px.nextSibling().toElement(); 273 | } 274 | } 275 | item = item.nextSibling().toElement(); 276 | } 277 | } 278 | child = child.nextSibling().toElement(); 279 | } 280 | } 281 | 282 | } // namespace pacpus 283 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/VelodyneInterface.h: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | 4 | @date created 2010-08-03 5 | @author Julien Moras 6 | */ 7 | 8 | #ifndef VELODYNEINTERFACE_H 9 | #define VELODYNEINTERFACE_H 10 | 11 | #include 12 | #include 13 | 14 | //#include "LibSensorComponent.h" 15 | #include "kernel/ComponentBase.h" 16 | #include "../VelodyneComponent/structure_velodyne.h" 17 | #include "structure_velodyne_cart.h" 18 | //#include "structure_IGN.h" 19 | 20 | 21 | #ifdef WIN32 22 | # ifdef VelodynePCLViewer_EXPORTS 23 | // make DLL 24 | # define SENSORCOMPONENT_API __declspec(dllexport) 25 | # else 26 | // use DLL 27 | # define SENSORCOMPONENT_API __declspec(dllimport) 28 | # endif 29 | #else 30 | // On other platforms, simply ignore this 31 | # define SENSORCOMPONENT_API 32 | #endif 33 | 34 | 35 | class QImage; 36 | 37 | namespace pacpus { 38 | 39 | class ShMem; 40 | 41 | struct SENSORCOMPONENT_API VelodyneComputingStrategy 42 | { 43 | virtual void processRaw(VelodynePolarData * polarScanData) = 0; 44 | virtual void processCorrected(VelodyneCartData * cartesianScanData) = 0; 45 | }; 46 | 47 | class SENSORCOMPONENT_API VelodyneInterface 48 | : public QThread 49 | , public ComponentBase 50 | { 51 | Q_OBJECT 52 | 53 | private: 54 | static const char * SHARED_MEMORY_NAME; 55 | 56 | public: 57 | static const char * COMPONENT_NAME; 58 | static const char * COMPONENT_XML_NAME; 59 | 60 | /** 61 | * Constructor 62 | * 63 | * @param name TODO 64 | */ 65 | VelodyneInterface(QString name); 66 | 67 | /** 68 | * Destructor 69 | */ 70 | virtual ~VelodyneInterface(); 71 | 72 | virtual void stopActivity(); 73 | virtual void startActivity(); 74 | virtual COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); 75 | 76 | void setVelodyneComputingStrategy(VelodyneComputingStrategy * component) {velodyneComputingStrategy =component;} 77 | 78 | protected: 79 | void run(); 80 | 81 | private: 82 | bool recording_; 83 | bool dbt2txt_; 84 | FILE * dbt2txtFile_; 85 | QString filePath_; 86 | bool m_isThreadAlive; 87 | 88 | static const int kVelodyneLayerCount = 64; 89 | double m_rotCor[kVelodyneLayerCount]; 90 | double m_vertCor[kVelodyneLayerCount]; 91 | double m_distCor[kVelodyneLayerCount]; 92 | double m_hOffsetCor[kVelodyneLayerCount]; 93 | double m_vOffsetCor[kVelodyneLayerCount]; 94 | 95 | void loadCorrections(const std::string & file); 96 | 97 | // The shared memory where data are provided 98 | ShMem * shmem_; 99 | QMutex mutex; 100 | //incoming LidarData 101 | VelodynePolarData velodyneData_; 102 | VelodyneCartData velodyneCartData_; 103 | 104 | VelodyneComputingStrategy * velodyneComputingStrategy; 105 | 106 | }; 107 | 108 | } // namespace pacpus 109 | 110 | #endif // VELODYNEINTERFACE_H 111 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/VelodynePCLViewerPlugin.cpp: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #include "VelodynePCLViewerPlugin.h" 5 | 6 | VelodynePCLViewerPlugin::VelodynePCLViewerPlugin() 7 | { 8 | } 9 | 10 | VelodynePCLViewerPlugin::~VelodynePCLViewerPlugin() 11 | { 12 | } 13 | 14 | QString VelodynePCLViewerPlugin::name() 15 | { 16 | return "VelodynePCLViewerPlugin"; 17 | } 18 | 19 | Q_EXPORT_PLUGIN2(VelodynePCLViewerPlugin, VelodynePCLViewerPlugin) 20 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/VelodynePCLViewerPlugin.h: -------------------------------------------------------------------------------- 1 | // Autogenerated file by PacpusPlugin.cmake 2 | // DO NOT EDIT!!! ALL CHANGES WOULD BE REMOVED BY THE NEXT CALL OF CMAKE 3 | 4 | #ifndef __VELODYNEPCLVIEWERPLUGIN_H__ 5 | #define __VELODYNEPCLVIEWERPLUGIN_H__ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /// Auto-generated plugin class 13 | class VelodynePCLViewerPlugin 14 | : public QObject 15 | , public PacpusPluginInterface 16 | { 17 | Q_OBJECT 18 | Q_INTERFACES(PacpusPluginInterface) 19 | 20 | public: 21 | VelodynePCLViewerPlugin(); 22 | ~VelodynePCLViewerPlugin(); 23 | 24 | protected: 25 | QString name(); 26 | }; 27 | 28 | #endif // __VELODYNEPCLVIEWERPLUGIN_H__ 29 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/XML/VelodynePCLViewer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/XML/VelodynePCLViewer_d.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | @file 3 | Purpose: CLD_Demo 4 | 5 | @date created 2009-11-03 16:13 6 | @version $Id: $ 7 | @author Julien Moras 8 | */ 9 | 10 | #include 11 | #ifndef WIN32 12 | # include 13 | #endif // WIN32 14 | #include 15 | 16 | #include "kernel/ComponentFactory.h" 17 | #include "kernel/ComponentManager.h" 18 | #include "kernel/Log.h" 19 | #include "ComputingComponent.h" 20 | 21 | using namespace pacpus; 22 | 23 | // ================================================================================ 24 | using ::boost::shared_ptr; 25 | 26 | // ================================================================================ 27 | DECLARE_STATIC_LOGGER("pacpus.cityvip.VelodynePCLViewer"); 28 | 29 | #ifdef NDEBUG 30 | static const QString kDefaultXmlConfigFilePath = "XML/VelodynePCLViewer.xml"; 31 | #else 32 | static const QString kDefaultXmlConfigFilePath = "XML/VelodynePCLViewer_d.xml"; 33 | #endif 34 | 35 | // ================================================================================ 36 | int main(int argc, char * argv[]) 37 | { 38 | #ifndef WIN32 39 | // configure logging facility 40 | ::log4cxx::BasicConfigurator::configure(); 41 | # ifdef NDEBUG 42 | ::log4cxx::Logger::getRootLogger()->setLevel(log4cxx::Level::getInfo()); 43 | # else 44 | ::log4cxx::Logger::getRootLogger()->setLevel(log4cxx::Level::getDebug()); 45 | # endif 46 | #endif // WIN32 47 | 48 | // ================================================================================ 49 | QApplication app(argc, argv); 50 | shared_ptr mgr(ComponentManager::getInstance(), ComponentManager::destroyer()); 51 | 52 | QString configFilePath; 53 | if (argc > 1) { 54 | configFilePath = argv[1]; 55 | } else { 56 | LOG_WARN("no XML config file specified. Using default: " << kDefaultXmlConfigFilePath); 57 | configFilePath = kDefaultXmlConfigFilePath; 58 | } 59 | LOG_INFO("loading file '" << configFilePath << "'"); 60 | mgr->loadComponents(configFilePath); 61 | 62 | // start components 63 | mgr->start("computingComponent"); 64 | mgr->start("velodyneInterface"); 65 | 66 | // save application exit status 67 | int exitStatus = app.exec(); 68 | 69 | // stop all components before exiting app 70 | mgr->stop(); 71 | 72 | // return application exit status 73 | return exitStatus; 74 | } 75 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/structure_velodyne_cart.h: -------------------------------------------------------------------------------- 1 | #ifndef STRUCTURE_VELODYNE_CART_H 2 | #define STRUCTURE_VELODYNE_CART_H 3 | 4 | #define VELODYNE_BLOCK_SIZE 100 5 | #define VELODYNE_PACKET_SIZE 1206 6 | #define VELODYNE_SCAN_SIZE 4166 7 | #define VELODYNE_NB_BLOCKS_PER_PACKET 12 8 | 9 | #pragma pack(push, 1) 10 | 11 | // 25 bytes size 12 | typedef struct VelodyneCartPoint 13 | { 14 | float distance; // in meter - if 0 no return up to 65m 15 | double X,Y,Z; // in meter if 0 no return up to 65m 16 | unsigned char intensity; // 255 most intense return 17 | }VelodyneCartPoint; 18 | 19 | // 100=VELODYNE_BLOCK_SIZE bytes size 20 | typedef struct VelodyneCartBlock 21 | { 22 | unsigned short block; // 0xEEFF upper and 0xDDFF lower 23 | float alpha; // azimuth in degrees 24 | float beta; // elevation in degrees 25 | struct VelodyneCartPoint Points[32]; // a couple of distance and intensity representing a point 26 | }VelodyneCartBlock; 27 | 28 | 29 | // size : 100*4166 + 2*4166 + 8 + 4 = 424 944 bytes 30 | // structure containing data of a complete revolution of the lidar 31 | typedef struct VelodyneCartData 32 | { 33 | VelodyneCartBlock Data[VELODYNE_SCAN_SIZE]; // data of one block of 32 beams (upper or lower) 34 | unsigned short scanCount[VELODYNE_SCAN_SIZE]; // scan count relative to polarData above 35 | road_time_t time; // time got in the packet containing first angle 36 | road_timerange_t timerange; // timerange = diff( t(angle=0) - t(lastangle) ) 37 | short range; // not all polarData are useful, use range to know until which index you can use the data 38 | }VelodyneCartData; 39 | 40 | #pragma pack(pop) 41 | 42 | #endif // STRUCTURE_VELODYNE_CART_H 43 | -------------------------------------------------------------------------------- /pacpussensors/tx_p12/ui/widgetPCL.cpp: -------------------------------------------------------------------------------- 1 | #include "widgetPCL.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | //#include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "kernel/Log.h" 18 | 19 | using namespace pacpus; 20 | 21 | const std::vector WidgetPCL::Default_Color(3, 255); 22 | 23 | DECLARE_STATIC_LOGGER("pacpus.cityvip.WidgetPCL"); 24 | 25 | WidgetPCL::WidgetPCL() 26 | { 27 | LOG_TRACE("constructor"); 28 | 29 | // renderWindow=NULL; 30 | 31 | /* 32 | firstTime =true; 33 | counter=true; 34 | received_new_cloud_data=false; 35 | */ 36 | 37 | start(); 38 | } 39 | 40 | WidgetPCL::~WidgetPCL() 41 | { 42 | LOG_TRACE("destructor"); 43 | 44 | delete viewer; 45 | } 46 | 47 | /* 48 | vtkSmartPointer WidgetPCL::getRenderWindows() { 49 | return renderWindow; 50 | } 51 | */ 52 | 53 | 54 | 55 | 56 | pcl::PolygonMesh reconstruct_polygonmesh(pcl::PointCloud::Ptr cloud){ 57 | pcl::NormalEstimation n; 58 | pcl::PointCloud::Ptr normals (new pcl::PointCloud); 59 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 60 | tree->setInputCloud (cloud); 61 | n.setInputCloud (cloud); 62 | n.setSearchMethod (tree); 63 | n.setKSearch (20); 64 | n.compute (*normals); 65 | //* normals should not contain the point normals + surface curvatures 66 | 67 | // Concatenate the XYZ and normal fields* 68 | pcl::PointCloud::Ptr cloud_with_normals (new pcl::PointCloud); 69 | pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); 70 | //* cloud_with_normals = cloud + normals 71 | 72 | // Create search tree* 73 | pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree); 74 | tree2->setInputCloud (cloud_with_normals); 75 | 76 | // Initialize objects 77 | pcl::GreedyProjectionTriangulation gp3; 78 | pcl::PolygonMesh triangles; 79 | 80 | // Set the maximum distance between connected points (maximum edge length) 81 | gp3.setSearchRadius (0.25); 82 | 83 | // Set typical values for the parameters 84 | gp3.setMu (2.5); 85 | gp3.setMaximumNearestNeighbors (100); 86 | gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees 87 | gp3.setMinimumAngle(M_PI/18); // 10 degrees 88 | gp3.setMaximumAngle(2*M_PI/3); // 120 degrees 89 | gp3.setNormalConsistency(false); 90 | 91 | // Get result 92 | gp3.setInputCloud (cloud_with_normals); 93 | gp3.setSearchMethod (tree2); 94 | gp3.reconstruct (triangles); 95 | 96 | return triangles; 97 | } 98 | 99 | 100 | void WidgetPCL::run() 101 | { 102 | // FIXME: deleted in destructor, move instantiation to constructor, otherwise memory leak 103 | viewer = new pcl::visualization::PCLVisualizer("3D Viewer"); 104 | 105 | viewer->setBackgroundColor (0, 0, 0); 106 | //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 107 | viewer->addCoordinateSystem (1.0); 108 | viewer->initCameraParameters (); 109 | 110 | /* 111 | renderWindow = viewer->getRenderWindow(); 112 | 113 | widget = new QVTKWidget(); 114 | widget->resize(512, 256); 115 | widget->SetRenderWindow(renderWindow); 116 | widget->setWindowTitle("ok"); 117 | widget->show(); 118 | */ 119 | 120 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 121 | cloud_xyz=cloud; 122 | 123 | while (!viewer->wasStopped()) { 124 | // sleep(1); 125 | /* 126 | if (received_new_cloud_data) { 127 | QMutexLocker mutexLocker(&mutex); 128 | Q_UNUSED(mutexLocker); 129 | 130 | received_new_cloud_data = false; 131 | 132 | viewer->removePointCloud("1"); 133 | viewer->addPointCloud(cloud_xyz,"1"); 134 | } 135 | */ 136 | 137 | while(!cloudPoint_queue.empty()) { 138 | QMutexLocker mutexLocker(&mutex); 139 | Q_UNUSED(mutexLocker); 140 | 141 | //received_new_cloud_data = false; 142 | 143 | viewer->removePointCloud(cloudPoint_queue.front().name); 144 | viewer->addPointCloud(cloudPoint_queue.front().cloud_xyz,cloudPoint_queue.front().color,cloudPoint_queue.front().name,cloudPoint_queue.front().viewport); 145 | 146 | //std::cerr << "size" << cloudPoint_queue.size() <<"Name " << cloudPoint_queue.front().name << "cloud size " << std::endl; 147 | cloudPoint_queue.pop(); 148 | } 149 | 150 | viewer->spinOnce(100); 151 | // qDebug() << "run finish" << cloud_xyz->width; 152 | } 153 | } 154 | 155 | /* 156 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz) 157 | { 158 | mutex.lock(); 159 | cloud_xyz=new_cloud_xyz; 160 | mutex.unlock(); 161 | received_new_cloud_data=true; 162 | qDebug() << "Updated" ; 163 | } 164 | 165 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, QString new_name, int viewport) 166 | { 167 | pcl::visualization::PointCloudColorHandlerCustom new_color = pcl::visualization::PointCloudColorHandlerCustom(new_cloud_xyz,255,255,255); 168 | updatePointCloud(new_cloud_xyz,new_color,new_name,viewport); 169 | } 170 | 171 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, pcl::visualization::PointCloudColorHandlerCustom new_color, QString new_name , int viewport) 172 | { 173 | CloudPointXYZDisplay CloudPoint ={new_cloud_xyz,new_color,new_name.toStdString(),viewport}; 174 | mutex.lock(); 175 | cloud_queue.push(new_cloud_xyz); 176 | cloud_name_queue.push(new_name); 177 | cloudPoint_queue.push(CloudPoint); 178 | mutex.unlock(); 179 | qDebug() << "Updated" ; 180 | } 181 | */ 182 | 183 | pcl::PointCloud::Ptr downsample_cloud(pcl::PointCloud::Ptr cloud){ 184 | pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud()); 185 | 186 | //Downsample 187 | pcl::VoxelGrid sor; 188 | sor.setInputCloud (cloud); 189 | sor.setLeafSize (0.1f, 0.1f, 0.1f); 190 | sor.filter (*cloud_filtered); 191 | 192 | //-> PointCloud before filtering: 133312 data points (x y z).PointCloud after filtering: 48027 data points (x y z).^C 193 | 194 | return cloud_filtered; 195 | } 196 | 197 | 198 | void WidgetPCL::updatePointCloud(pcl::PointCloud::Ptr new_cloud_xyz, QString new_name, std::vector new_color_val, int viewport) 199 | { 200 | LOG_TRACE("updating point cloud..."); 201 | 202 | new_cloud_xyz = downsample_cloud(new_cloud_xyz); 203 | 204 | pcl::visualization::PointCloudColorHandlerCustom new_color = 205 | pcl::visualization::PointCloudColorHandlerCustom(new_cloud_xyz, new_color_val[0], new_color_val[1], new_color_val[2]); 206 | CloudPointXYZDisplay CloudPoint = {new_cloud_xyz, new_color, new_name.toStdString(), viewport}; 207 | 208 | QMutexLocker mutexLocker(&mutex); 209 | Q_UNUSED(mutexLocker); 210 | 211 | /*cloud_queue.push(new_cloud_xyz); 212 | cloud_name_queue.push(new_name);*/ 213 | cloudPoint_queue.push(CloudPoint); 214 | 215 | LOG_DEBUG("updated point cloud"); 216 | } 217 | 218 | /* 219 | void WidgetPCL::UpdatePointCloud(pcl::PointCloud::ConstPtr cloud) { 220 | 221 | if (!viewer->addPointCloud (cloud, "sample cloud")) 222 | viewer->addPointCloud (cloud, "sample cloud"); 223 | viewer->getRenderWindow(); 224 | } 225 | */ -------------------------------------------------------------------------------- /pacpussensors/tx_p12/ui/widgetPCL.h: -------------------------------------------------------------------------------- 1 | #ifndef WIDGETPCL_H 2 | #define WIDGETPCL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | //#include "vtkSmartPointer.h" 14 | //#include "vtkRenderWindow.h" 15 | 16 | //class QVTKWidget; 17 | 18 | namespace pcl { namespace visualization { 19 | class PCLVisualizer; 20 | } } 21 | 22 | struct CloudPointXYZDisplay 23 | { 24 | pcl::PointCloud::Ptr cloud_xyz; 25 | pcl::visualization::PointCloudColorHandlerCustom color; 26 | std::string name; 27 | int viewport; 28 | }; 29 | 30 | namespace pacpus { 31 | 32 | class WidgetPCL 33 | : public QThread 34 | { 35 | Q_OBJECT 36 | 37 | public: 38 | WidgetPCL(); 39 | ~WidgetPCL(); 40 | 41 | static const std::vector Default_Color; 42 | 43 | //vtkSmartPointer getRenderWindows(); 44 | //void updatePointCloud(pcl::PointCloud::Ptr); 45 | //void updatePointCloud(pcl::PointCloud::Ptr, QString = "cloud", int = 0); 46 | //void updatePointCloud(pcl::PointCloud::Ptr, pcl::visualization::PointCloudColorHandlerCustom ,QString = "cloud", int = 0); 47 | void updatePointCloud(pcl::PointCloud::Ptr, QString = "cloud", std::vector = Default_Color, int = 0); 48 | //QVTKWidget * widget; 49 | 50 | protected: 51 | void run(); 52 | 53 | private: 54 | std::queue cloudPoint_queue; 55 | pcl::PointCloud::Ptr cloud_xyz; 56 | std::queue::Ptr> cloud_queue; 57 | pcl::visualization::PCLVisualizer * viewer; 58 | QString name; 59 | std::queue cloud_name_queue; 60 | 61 | //vtkSmartPointer renderWindow; 62 | //bool firstTime,counter,received_new_cloud_data; 63 | QMutex mutex; 64 | }; 65 | 66 | } 67 | 68 | #endif // WIDGETPCL_H 69 | -------------------------------------------------------------------------------- /rapport.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mdamien/velodyne/301033244859c52f75a380f45e30df365324afa2/rapport.pdf -------------------------------------------------------------------------------- /rapport.tex: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mdamien/velodyne/301033244859c52f75a380f45e30df365324afa2/rapport.tex -------------------------------------------------------------------------------- /tests/bunny.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v.5 - Point Cloud Data file format 2 | VERSION .5 3 | FIELDS x y z 4 | SIZE 4 4 4 5 | TYPE F F F 6 | COUNT 1 1 1 7 | WIDTH 397 8 | HEIGHT 1 9 | POINTS 397 10 | DATA ascii 11 | 0.0054216 0.11349 0.040749 12 | -0.0017447 0.11425 0.041273 13 | -0.010661 0.11338 0.040916 14 | 0.026422 0.11499 0.032623 15 | 0.024545 0.12284 0.024255 16 | 0.034137 0.11316 0.02507 17 | 0.02886 0.11773 0.027037 18 | 0.02675 0.12234 0.017605 19 | 0.03575 0.1123 0.019109 20 | 0.015982 0.12307 0.031279 21 | 0.0079813 0.12438 0.032798 22 | 0.018101 0.11674 0.035493 23 | 0.0086687 0.11758 0.037538 24 | 0.01808 0.12536 0.026132 25 | 0.0080861 0.12866 0.02619 26 | 0.02275 0.12146 0.029671 27 | -0.0018689 0.12456 0.033184 28 | -0.011168 0.12376 0.032519 29 | -0.0020063 0.11937 0.038104 30 | -0.01232 0.11816 0.037427 31 | -0.0016659 0.12879 0.026782 32 | -0.011971 0.12723 0.026219 33 | 0.016484 0.12828 0.01928 34 | 0.0070921 0.13103 0.018415 35 | 0.0014615 0.13134 0.017095 36 | -0.013821 0.12886 0.019265 37 | -0.01725 0.11202 0.040077 38 | -0.074556 0.13415 0.051046 39 | -0.065971 0.14396 0.04109 40 | -0.071925 0.14545 0.043266 41 | -0.06551 0.13624 0.042195 42 | -0.071112 0.13767 0.047518 43 | -0.079528 0.13416 0.051194 44 | -0.080421 0.14428 0.042793 45 | -0.082672 0.1378 0.046806 46 | -0.08813 0.13514 0.042222 47 | -0.066325 0.12347 0.050729 48 | -0.072399 0.12662 0.052364 49 | -0.066091 0.11973 0.050881 50 | -0.072012 0.11811 0.052295 51 | -0.062433 0.12627 0.043831 52 | -0.068326 0.12998 0.048875 53 | -0.063094 0.11811 0.044399 54 | -0.071301 0.11322 0.04841 55 | -0.080515 0.12741 0.052034 56 | -0.078179 0.1191 0.051116 57 | -0.085216 0.12609 0.049001 58 | -0.089538 0.12621 0.044589 59 | -0.082659 0.11661 0.04797 60 | -0.089536 0.11784 0.04457 61 | -0.0565 0.15248 0.030132 62 | -0.055517 0.15313 0.026915 63 | -0.03625 0.17198 0.00017688 64 | -0.03775 0.17198 0.00022189 65 | -0.03625 0.16935 0.00051958 66 | -0.033176 0.15711 0.0018682 67 | -0.051913 0.1545 0.011273 68 | -0.041707 0.16642 0.0030522 69 | -0.049468 0.16414 0.0041988 70 | -0.041892 0.15669 0.0054879 71 | -0.051224 0.15878 0.0080283 72 | -0.062417 0.15317 0.033161 73 | -0.07167 0.15319 0.033701 74 | -0.062543 0.15524 0.027405 75 | -0.07211 0.1555 0.027645 76 | -0.078663 0.15269 0.032268 77 | -0.081569 0.15374 0.026085 78 | -0.08725 0.1523 0.022135 79 | -0.05725 0.15568 0.010325 80 | -0.057888 0.1575 0.0073225 81 | -0.0885 0.15223 0.019215 82 | -0.056129 0.14616 0.03085 83 | -0.054705 0.13555 0.032127 84 | -0.054144 0.14714 0.026275 85 | -0.046625 0.13234 0.021909 86 | -0.05139 0.13694 0.025787 87 | -0.018278 0.12238 0.030773 88 | -0.021656 0.11643 0.035209 89 | -0.031921 0.11566 0.032851 90 | -0.021348 0.12421 0.024562 91 | -0.03241 0.12349 0.023293 92 | -0.024869 0.12094 0.028745 93 | -0.031747 0.12039 0.028229 94 | -0.052912 0.12686 0.034968 95 | -0.041672 0.11564 0.032998 96 | -0.052037 0.1168 0.034582 97 | -0.042495 0.12488 0.024082 98 | -0.047946 0.12736 0.028108 99 | -0.042421 0.12035 0.028633 100 | -0.047661 0.12024 0.028871 101 | -0.035964 0.1513 0.0005395 102 | -0.050598 0.1474 0.013881 103 | -0.046375 0.13293 0.018289 104 | -0.049125 0.13856 0.016269 105 | -0.042976 0.14915 0.0054003 106 | -0.047965 0.14659 0.0086783 107 | -0.022926 0.1263 0.018077 108 | -0.031583 0.1259 0.017804 109 | -0.041733 0.12796 0.01665 110 | -0.061482 0.14698 0.036168 111 | -0.071729 0.15026 0.038328 112 | -0.060526 0.1368 0.035999 113 | -0.082619 0.14823 0.035955 114 | -0.087824 0.14449 0.033779 115 | -0.089 0.13828 0.037774 116 | -0.085662 0.15095 0.028208 117 | -0.089601 0.14725 0.025869 118 | -0.090681 0.13748 0.02369 119 | -0.058722 0.12924 0.038992 120 | -0.060075 0.11512 0.037685 121 | -0.091812 0.12767 0.038703 122 | -0.091727 0.11657 0.039619 123 | -0.093164 0.12721 0.025211 124 | -0.093938 0.12067 0.024399 125 | -0.091583 0.14522 0.01986 126 | -0.090929 0.13667 0.019817 127 | -0.093094 0.11635 0.018959 128 | 0.024948 0.10286 0.041418 129 | 0.0336 0.092627 0.040463 130 | 0.02742 0.096386 0.043312 131 | 0.03392 0.086911 0.041034 132 | 0.028156 0.086837 0.045084 133 | 0.03381 0.078604 0.040854 134 | 0.028125 0.076874 0.045059 135 | 0.0145 0.093279 0.05088 136 | 0.0074817 0.09473 0.052315 137 | 0.017407 0.10535 0.043139 138 | 0.0079536 0.10633 0.042968 139 | 0.018511 0.097194 0.047253 140 | 0.0086436 0.099323 0.048079 141 | -0.0020197 0.095698 0.053906 142 | -0.011446 0.095169 0.053862 143 | -0.001875 0.10691 0.043455 144 | -0.011875 0.10688 0.043019 145 | -0.0017622 0.10071 0.046648 146 | -0.012498 0.10008 0.045916 147 | 0.016381 0.085894 0.051642 148 | 0.0081167 0.08691 0.055228 149 | 0.017644 0.076955 0.052372 150 | 0.008125 0.076853 0.055536 151 | 0.020575 0.088169 0.049006 152 | 0.022445 0.075721 0.049563 153 | -0.0017931 0.086849 0.056843 154 | -0.011943 0.086771 0.057009 155 | -0.0019567 0.076863 0.057803 156 | -0.011875 0.076964 0.057022 157 | 0.03325 0.067541 0.040033 158 | 0.028149 0.066829 0.042953 159 | 0.026761 0.057829 0.042588 160 | 0.023571 0.04746 0.040428 161 | 0.015832 0.067418 0.051639 162 | 0.0080431 0.066902 0.055006 163 | 0.013984 0.058886 0.050416 164 | 0.0080973 0.056888 0.05295 165 | 0.020566 0.065958 0.0483 166 | 0.018594 0.056539 0.047879 167 | 0.012875 0.052652 0.049689 168 | -0.0017852 0.066712 0.056503 169 | -0.011785 0.066885 0.055015 170 | -0.001875 0.056597 0.05441 171 | -0.01184 0.057054 0.052714 172 | -0.015688 0.052469 0.049615 173 | 0.0066154 0.04993 0.051259 174 | 0.018088 0.046655 0.043321 175 | 0.008841 0.045437 0.046623 176 | 0.017688 0.039719 0.043084 177 | 0.008125 0.039516 0.045374 178 | -0.0016111 0.049844 0.05172 179 | -0.01245 0.046773 0.050903 180 | -0.013851 0.039778 0.051036 181 | -0.0020294 0.044874 0.047587 182 | -0.011653 0.04686 0.048661 183 | -0.0018611 0.039606 0.047339 184 | -0.0091545 0.03958 0.049415 185 | 0.043661 0.094028 0.02252 186 | 0.034642 0.10473 0.031831 187 | 0.028343 0.1072 0.036339 188 | 0.036339 0.096552 0.034843 189 | 0.031733 0.099372 0.038505 190 | 0.036998 0.10668 0.026781 191 | 0.032875 0.11108 0.02959 192 | 0.040938 0.097132 0.026663 193 | 0.044153 0.086466 0.024241 194 | 0.05375 0.072221 0.020429 195 | 0.04516 0.076574 0.023594 196 | 0.038036 0.086663 0.035459 197 | 0.037861 0.076625 0.035658 198 | 0.042216 0.087237 0.028254 199 | 0.042355 0.076747 0.02858 200 | 0.043875 0.096228 0.015269 201 | 0.044375 0.096797 0.0086445 202 | 0.039545 0.1061 0.017655 203 | 0.042313 0.10009 0.017237 204 | 0.045406 0.087417 0.015604 205 | 0.055118 0.072639 0.017944 206 | 0.048722 0.07376 0.017434 207 | 0.045917 0.086298 0.0094211 208 | 0.019433 0.1096 0.039063 209 | 0.01097 0.11058 0.039648 210 | 0.046657 0.057153 0.031337 211 | 0.056079 0.066335 0.024122 212 | 0.048168 0.06701 0.026298 213 | 0.056055 0.057253 0.024902 214 | 0.051163 0.056662 0.029137 215 | 0.036914 0.067032 0.036122 216 | 0.033 0.06472 0.039903 217 | 0.038004 0.056507 0.033119 218 | 0.030629 0.054915 0.038484 219 | 0.041875 0.066383 0.028357 220 | 0.041434 0.06088 0.029632 221 | 0.044921 0.049904 0.031243 222 | 0.054635 0.050167 0.022044 223 | 0.04828 0.04737 0.025845 224 | 0.037973 0.048347 0.031456 225 | 0.028053 0.047061 0.035991 226 | 0.025595 0.040346 0.03415 227 | 0.038455 0.043509 0.028278 228 | 0.032031 0.043278 0.029253 229 | 0.036581 0.040335 0.025144 230 | 0.03019 0.039321 0.026847 231 | 0.059333 0.067891 0.017361 232 | 0.0465 0.071452 0.01971 233 | 0.059562 0.057747 0.01834 234 | 0.055636 0.049199 0.019173 235 | 0.0505 0.045064 0.019181 236 | 0.023 0.047803 0.039776 237 | 0.022389 0.03886 0.038795 238 | -0.019545 0.0939 0.052205 239 | -0.021462 0.10618 0.042059 240 | -0.031027 0.10395 0.041228 241 | -0.022521 0.097723 0.045194 242 | -0.031858 0.097026 0.043878 243 | -0.043262 0.10412 0.040891 244 | -0.052154 0.10404 0.040972 245 | -0.041875 0.096944 0.042424 246 | -0.051919 0.096967 0.043563 247 | -0.021489 0.086672 0.054767 248 | -0.027 0.083087 0.050284 249 | -0.02107 0.077249 0.054365 250 | -0.026011 0.089634 0.048981 251 | -0.031893 0.087035 0.044169 252 | -0.025625 0.074892 0.047102 253 | -0.03197 0.0769 0.042177 254 | -0.041824 0.086954 0.043295 255 | -0.051825 0.086844 0.044933 256 | -0.041918 0.076728 0.042564 257 | -0.051849 0.076877 0.042992 258 | -0.061339 0.10393 0.041164 259 | -0.072672 0.10976 0.044294 260 | -0.061784 0.096825 0.043327 261 | -0.070058 0.096203 0.041397 262 | -0.080439 0.11091 0.044343 263 | -0.061927 0.086724 0.04452 264 | -0.070344 0.087352 0.041908 265 | -0.06141 0.077489 0.042178 266 | -0.068579 0.080144 0.041024 267 | -0.019045 0.067732 0.052388 268 | -0.017742 0.058909 0.050809 269 | -0.023548 0.066382 0.045226 270 | -0.03399 0.067795 0.040929 271 | -0.02169 0.056549 0.045164 272 | -0.036111 0.060706 0.040407 273 | -0.041231 0.066951 0.041392 274 | -0.048588 0.070956 0.040357 275 | -0.0403 0.059465 0.040446 276 | -0.02192 0.044965 0.052258 277 | -0.029187 0.043585 0.051088 278 | -0.021919 0.039826 0.053521 279 | -0.030331 0.039749 0.052133 280 | -0.021998 0.049847 0.046725 281 | -0.031911 0.046848 0.045187 282 | -0.035276 0.039753 0.047529 283 | -0.042016 0.044823 0.041594 284 | -0.05194 0.044707 0.043498 285 | -0.041928 0.039327 0.043582 286 | -0.051857 0.039252 0.046212 287 | -0.059453 0.04424 0.042862 288 | -0.060765 0.039087 0.044363 289 | -0.024273 0.11038 0.039129 290 | -0.032379 0.10878 0.037952 291 | -0.041152 0.10853 0.037969 292 | -0.051698 0.10906 0.038258 293 | -0.062091 0.10877 0.038274 294 | -0.071655 0.10596 0.037516 295 | -0.074634 0.097746 0.038347 296 | -0.07912 0.10508 0.032308 297 | -0.080203 0.096758 0.033592 298 | -0.08378 0.10568 0.025985 299 | -0.087292 0.10314 0.020825 300 | -0.08521 0.097079 0.02781 301 | -0.088082 0.096456 0.022985 302 | -0.07516 0.08604 0.038816 303 | -0.064577 0.073455 0.03897 304 | -0.072279 0.076416 0.036413 305 | -0.076375 0.072563 0.02873 306 | -0.080031 0.087076 0.03429 307 | -0.078919 0.079371 0.032477 308 | -0.084834 0.086686 0.026974 309 | -0.087891 0.089233 0.022611 310 | -0.081048 0.077169 0.025829 311 | -0.086393 0.10784 0.018635 312 | -0.087672 0.10492 0.017264 313 | -0.089333 0.098483 0.01761 314 | -0.086375 0.083067 0.018607 315 | -0.089179 0.089186 0.018947 316 | -0.082879 0.076109 0.017794 317 | -0.0825 0.074674 0.0071175 318 | -0.026437 0.064141 0.039321 319 | -0.030035 0.06613 0.038942 320 | -0.026131 0.056531 0.038882 321 | -0.031664 0.056657 0.037742 322 | -0.045716 0.064541 0.039166 323 | -0.051959 0.066869 0.036733 324 | -0.042557 0.055545 0.039026 325 | -0.049406 0.056892 0.034344 326 | -0.0555 0.062391 0.029498 327 | -0.05375 0.058574 0.026313 328 | -0.03406 0.050137 0.038577 329 | -0.041741 0.04959 0.03929 330 | -0.050975 0.049435 0.036965 331 | -0.053 0.051065 0.029209 332 | -0.054145 0.054568 0.012257 333 | -0.055848 0.05417 0.0083272 334 | -0.054844 0.049295 0.011462 335 | -0.05615 0.050619 0.0092929 336 | -0.061451 0.068257 0.035376 337 | -0.069725 0.069958 0.032788 338 | -0.062823 0.063322 0.026886 339 | -0.071037 0.066787 0.025228 340 | -0.060857 0.060568 0.022643 341 | -0.067 0.061558 0.020109 342 | -0.0782 0.071279 0.021032 343 | -0.062116 0.045145 0.037802 344 | -0.065473 0.039513 0.037964 345 | -0.06725 0.03742 0.033413 346 | -0.072702 0.065008 0.018701 347 | -0.06145 0.059165 0.018731 348 | -0.0675 0.061479 0.019221 349 | -0.057411 0.054114 0.0038257 350 | -0.079222 0.070654 0.017735 351 | -0.062473 0.04468 0.01111 352 | -0.06725 0.042258 0.010414 353 | -0.066389 0.040515 0.01316 354 | -0.068359 0.038502 0.011958 355 | -0.061381 0.04748 0.007607 356 | -0.068559 0.043549 0.0081576 357 | -0.070929 0.03983 0.0085888 358 | -0.016625 0.18375 -0.019735 359 | -0.015198 0.17471 -0.018868 360 | -0.015944 0.16264 -0.0091037 361 | -0.015977 0.1607 -0.0088072 362 | -0.013251 0.16708 -0.015264 363 | -0.014292 0.16098 -0.011252 364 | -0.013986 0.184 -0.023739 365 | -0.011633 0.17699 -0.023349 366 | -0.0091029 0.16988 -0.021457 367 | -0.025562 0.18273 -0.0096247 368 | -0.02725 0.18254 -0.0094384 369 | -0.025736 0.17948 -0.0089653 370 | -0.031216 0.17589 -0.0051154 371 | -0.020399 0.1845 -0.014943 372 | -0.021339 0.17645 -0.014566 373 | -0.027125 0.17234 -0.010156 374 | -0.03939 0.1733 -0.0023575 375 | -0.022876 0.16406 -0.0078103 376 | -0.031597 0.16651 -0.0049292 377 | -0.0226 0.15912 -0.003799 378 | -0.030372 0.15767 -0.0012672 379 | -0.021158 0.16849 -0.012383 380 | -0.027 0.1712 -0.01022 381 | -0.041719 0.16813 -0.00074958 382 | -0.04825 0.16748 -0.00015191 383 | -0.03725 0.16147 -7.2628e-05 384 | -0.066429 0.15783 -0.0085673 385 | -0.071284 0.15839 -0.005998 386 | -0.065979 0.16288 -0.017792 387 | -0.071623 0.16384 -0.01576 388 | -0.066068 0.16051 -0.013567 389 | -0.073307 0.16049 -0.011832 390 | -0.077 0.16204 -0.019241 391 | -0.077179 0.15851 -0.01495 392 | -0.073691 0.17286 -0.037944 393 | -0.07755 0.17221 -0.039175 394 | -0.065921 0.16586 -0.025022 395 | -0.072095 0.16784 -0.024725 396 | -0.066 0.16808 -0.030916 397 | -0.073448 0.17051 -0.032045 398 | -0.07777 0.16434 -0.025938 399 | -0.077893 0.16039 -0.021299 400 | -0.078211 0.169 -0.034566 401 | -0.034667 0.15131 -0.00071029 402 | -0.066117 0.17353 -0.047453 403 | -0.071986 0.17612 -0.045384 404 | -0.06925 0.182 -0.055026 405 | -0.064992 0.17802 -0.054645 406 | -0.069935 0.17983 -0.051988 407 | -0.07793 0.17516 -0.0444 408 | -------------------------------------------------------------------------------- /tests/test_floor_detection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | using namespace pcl; 16 | 17 | int 18 | main (int argc, char** argv) 19 | { 20 | if (argc != 3) 21 | { 22 | PCL_ERROR ("Syntax: %s input.pcd output.stl \n", argv[0]); 23 | return -1; 24 | } 25 | 26 | PointCloud::Ptr cloud (new PointCloud ()) 27 | , cloud_p (new pcl::PointCloud) 28 | , cloud_p_neg (new pcl::PointCloud); 29 | io::loadPCDFile (argv[1], *cloud); 30 | 31 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 32 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 33 | pcl::SACSegmentation seg; 34 | seg.setOptimizeCoefficients (true); 35 | seg.setModelType (pcl::SACMODEL_PLANE); 36 | seg.setMethodType (pcl::SAC_RANSAC); 37 | seg.setDistanceThreshold (0.01); 38 | seg.setInputCloud ((*cloud).makeShared ()); 39 | seg.segment (*inliers, *coefficients); 40 | 41 | if (inliers->indices.size () == 0) 42 | { 43 | PCL_ERROR ("Could not estimate a planar model for the given dataset."); 44 | return (-1); 45 | } 46 | 47 | std::cerr << "Model coefficients: " << coefficients->values[0] << " " 48 | << coefficients->values[1] << " " 49 | << coefficients->values[2] << " " 50 | << coefficients->values[3] << std::endl; 51 | 52 | std::cerr << "Model inliers: " << inliers->indices.size () << std::endl; 53 | 54 | pcl::ExtractIndices extract; 55 | extract.setInputCloud (cloud); 56 | extract.setIndices (inliers); 57 | extract.setNegative (false); 58 | extract.filter (*cloud_p); 59 | 60 | extract.setInputCloud (cloud); 61 | extract.setIndices (inliers); 62 | extract.setNegative (true); 63 | extract.filter (*cloud_p_neg); 64 | 65 | boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 66 | viewer->setBackgroundColor (0, 0, 0); 67 | viewer->addPointCloud (cloud, "cloud"); 68 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); 69 | viewer->addCoordinateSystem (1.0); 70 | viewer->initCameraParameters (); 71 | viewer->addPlane(*coefficients, "plane"); 72 | 73 | while (!viewer->wasStopped ()) 74 | { 75 | viewer->spinOnce (100); 76 | boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 77 | } 78 | 79 | return (0); 80 | } 81 | -------------------------------------------------------------------------------- /tests/test_meshing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | void greedy_proj () { 21 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 22 | pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ()); 23 | pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); 24 | pcl::io::loadPCDFile ("input.pcd", *cloud_blob); 25 | pcl::fromPCLPointCloud2 (*cloud_blob, *cloud); 26 | 27 | cloud_blob = cloud_filtered; 28 | 29 | // Normal estimation 30 | pcl::NormalEstimation n; 31 | pcl::PointCloud::Ptr normals (new pcl::PointCloud); 32 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 33 | tree->setInputCloud (cloud); 34 | n.setInputCloud (cloud); 35 | n.setSearchMethod (tree); 36 | n.setKSearch (20); 37 | n.compute (*normals); 38 | 39 | // Concatenate the XYZ and normal fields 40 | pcl::PointCloud::Ptr cloud_with_normals (new pcl::PointCloud); 41 | pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); 42 | 43 | // Create search tree 44 | pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree); 45 | tree2->setInputCloud (cloud_with_normals); 46 | 47 | // Initialize objects 48 | pcl::GreedyProjectionTriangulation gp3; 49 | pcl::PolygonMesh triangles; 50 | 51 | // Set the maximum distance between connected points (maximum edge length) 52 | gp3.setSearchRadius (1); 53 | 54 | // Set typical values for the parameters 55 | gp3.setMu (2.5); 56 | gp3.setMaximumNearestNeighbors (10); 57 | gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees 58 | gp3.setMinimumAngle(M_PI/18); // 10 degrees 59 | gp3.setMaximumAngle(2*M_PI/3); // 120 degrees 60 | gp3.setNormalConsistency(false); 61 | 62 | // Get result 63 | gp3.setInputCloud (cloud_with_normals); 64 | gp3.setSearchMethod (tree2); 65 | gp3.reconstruct (triangles); 66 | 67 | // Additional vertex information 68 | std::vector parts = gp3.getPartIDs(); 69 | std::vector states = gp3.getPointStates(); 70 | 71 | pcl::io::saveVTKFile("output.vtk", triangles); 72 | pcl::io::savePolygonFileSTL("output.stl", triangles); 73 | } 74 | 75 | int mls() 76 | { 77 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); 78 | pcl::io::loadPCDFile ("input.pcd", *cloud); 79 | 80 | // Create a KD-Tree 81 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 82 | 83 | // Output has the PointNormal type in order to store the normals calculated by MLS 84 | pcl::PointCloud mls_points; 85 | 86 | // Init object (second point type is for the normals, even if unused) 87 | pcl::MovingLeastSquares mls; 88 | 89 | mls.setComputeNormals (true); 90 | 91 | // Set parameters 92 | mls.setInputCloud (cloud); 93 | mls.setPolynomialFit (true); 94 | mls.setSearchMethod (tree); 95 | mls.setSearchRadius (0.03); 96 | 97 | // Reconstruct 98 | mls.process (mls_points); 99 | 100 | // Save output 101 | pcl::io::savePCDFile ("output.pcd", mls_points); 102 | 103 | pcl_visualization::CloudViewer viewer("Simple Cloud Viewer"); 104 | viewer.showCloud(mls_points); 105 | while (!viewer.wasStopped()) 106 | { 107 | } 108 | } 109 | 110 | 111 | 112 | int main (int argc, char** argv){ 113 | //greedy_proj(); 114 | mls(); 115 | return 0; 116 | } -------------------------------------------------------------------------------- /tests/test_poisson.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | 15 | using namespace pcl; 16 | 17 | int main (int argc, char **argv) 18 | { 19 | if (argc != 3) 20 | { 21 | PCL_ERROR ("Syntax: %s input.pcd output.stl \n", argv[0]); 22 | return -1; 23 | } 24 | 25 | PointCloud::Ptr cloud (new PointCloud ()); 26 | io::loadPCDFile (argv[1], *cloud); 27 | 28 | MovingLeastSquares mls; 29 | mls.setInputCloud (cloud); 30 | mls.setSearchRadius (0.1); 31 | mls.setPolynomialFit (true); 32 | mls.setPolynomialOrder (2); 33 | mls.setUpsamplingMethod (MovingLeastSquares::SAMPLE_LOCAL_PLANE); 34 | mls.setUpsamplingRadius (0.05); 35 | mls.setUpsamplingStepSize (0.02); 36 | 37 | PointCloud::Ptr cloud_smoothed (new PointCloud ()); 38 | mls.process (*cloud_smoothed); 39 | NormalEstimationOMP ne; 40 | ne.setNumberOfThreads (8); 41 | ne.setInputCloud (cloud_smoothed); 42 | ne.setRadiusSearch (0.03); 43 | Eigen::Vector4f centroid; 44 | compute3DCentroid (*cloud_smoothed, centroid); 45 | ne.setViewPoint (centroid[0], centroid[1], centroid[2]); 46 | PointCloud::Ptr cloud_normals (new PointCloud ()); 47 | ne.compute (*cloud_normals); 48 | PointCloud::Ptr cloud_smoothed_normals (new PointCloud ()); 49 | concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals); 50 | 51 | Poisson poisson; 52 | poisson.setDepth (9); 53 | poisson.setInputCloud 54 | (cloud_smoothed_normals); 55 | 56 | PolygonMesh mesh; 57 | poisson.reconstruct (mesh); 58 | 59 | visualization::CloudViewer viewer("Simple Cloud Viewer"); 60 | viewer.showCloud(cloud); 61 | while (!viewer.wasStopped()) 62 | { 63 | } 64 | 65 | pcl::io::savePolygonFileSTL(argv[2], mesh); 66 | return 0; 67 | } -------------------------------------------------------------------------------- /xml/compute.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /xml/conf.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /xml/velodyne.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | --------------------------------------------------------------------------------