shmem_layer.cc
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           WMPSNIFFER          --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: shmem_layer.cc
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2012, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 #include <stdio.h>
00038 #include <sys/types.h>
00039 #include <sys/ipc.h>
00040 #include <sys/shm.h>
00041 #include <stdio.h>
00042 #include <semaphore.h>
00043 #include <sys/types.h>
00044 #include <sys/stat.h>
00045 #include <fcntl.h>
00046 #include "shmem_layer.h"
00047 
00048 #define SHMSZ 2500
00049 
00050 static int shmem_keep_running = 1;
00051 static char *shm;
00052 static sem_t * mutex,*mutex2;
00053 static int shmid, shmid2, nnodes, *sniff_present;
00054 static key_t key2 = 0x1111;
00055 
00056 int shmem_pre_init(){
00057         /* sniffer present */
00058         shmid2 = shmget(key2, sizeof(int), IPC_CREAT | 0666);
00059         if (shmid2 < 0) {
00060                 perror("failure in shmget");
00061                 exit(-1);
00062         }
00063          sniff_present = (int*) shmat(shmid2, NULL, 0);
00064         (*sniff_present) = 0x6789;
00065         return 0;
00066 }
00067 
00068 int shmem_init(int _nnodes){
00069 
00070         nnodes = _nnodes;
00071         key_t key = 0x6969;
00072 
00073         mutex = sem_open("wmp_sem_sniff", O_CREAT, 0644, 0);
00074         mutex2 = sem_open("wmp_sem_sniff2", O_CREAT, 0644, 0);
00075         if (mutex == SEM_FAILED) {
00076                 perror("unable to create semaphore");
00077                 sem_unlink("wmp_sem_sniff");
00078                 exit(-1);
00079         }
00080 
00081         /* shared memory allocation */
00082         shmid = shmget(key, SHMSZ, IPC_CREAT | 0666);
00083         if (shmid < 0) {
00084                 perror("failure in shmget");
00085                 exit(-1);
00086         }
00087         shm = (char*) shmat(shmid, NULL, 0);
00088 
00089 
00090         /* sniffer present */
00091         shmid2 = shmget(key2, sizeof(int), IPC_CREAT | 0666);
00092         if (shmid2 < 0) {
00093                 perror("failure in shmget");
00094                 exit(-1);
00095         }
00096          sniff_present = (int*) shmat(shmid2, NULL, 0);
00097         (*sniff_present) = 0x6789;
00098 
00099         while (!sem_trywait(mutex)){}
00100         while (!sem_trywait(mutex2)){}
00101 
00102         shmem_keep_running = 1;
00103         return 1;
00104 }
00105 
00106 
00107 int shmem_sniff_packet(char * data, simData_Hdr & sd, unsigned long long &time_us, std::map<int,robo_pose_t> & poses) {
00108         int ret, *len, nbytes;
00109 
00110         (*sniff_present) = 0x6789;
00111         if (shmem_keep_running){
00112 
00113                 ret = sem_wait(mutex);
00114 
00115                 len = (int*) shm;
00116                 nbytes = (*len);
00117                 if (nbytes <= 0) {
00118                         return 0;
00119                 }
00120                 memcpy(data, shm + sizeof(int), nbytes);
00121 
00122                 time_us=getActualTimeus();
00123                 sd.time=time_us;
00124                 sd.len=nbytes;
00125                 sd.num_nodes=nnodes;
00126                 sd.is_wmp=1;
00127                 sd.data_src = 32;
00128                 sd.frame_type=SP_LUS_WMP_FRAME;
00129             wmpFrame * f = (wmpFrame*) data;
00130                 poses[f->hdr.from].reached=true;
00131 
00132                 sem_post(mutex2);
00133 
00134                 return nbytes;
00135         }
00136         return 0;
00137 }
00138 
00139 int shmem_close(){
00140         shmem_keep_running = 0;
00141         return 0;
00142 }


ros_rt_wmp_sniffer
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:57