~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/novatelINS/src/changedOrcaFiles/gbxserialacfr/lockfile/lockfile.cpp

  • Committer: Nick Hillier
  • Date: 2011-08-03 03:16:26 UTC
  • Revision ID: nick.hillier@csiro.au-20110803031626-3ekp14jxlbda9bzf
Initial add of ROS wrappers for novatel INS around the Orca driver.

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
/*
 
2
 * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
 
3
 *               http://gearbox.sf.net/
 
4
 * Copyright (c) 2004-2008 Alex Brooks
 
5
 *
 
6
 * This distribution is licensed to you under the terms described in
 
7
 * the LICENSE file included in this distribution.
 
8
 *
 
9
 */
 
10
 
 
11
/*
 
12
 * Modified by Polly Alexander and Nick Hillier
 
13
 * 2009-2011
 
14
 */
 
15
 
 
16
#include "lockfile.h"
 
17
#include <cstring>
 
18
#include <cstdlib>
 
19
#include <iostream>
 
20
#include <signal.h>
 
21
#include <sys/types.h>
 
22
#include <errno.h>
 
23
#include <sstream>
 
24
#include <libgen.h>
 
25
#include <stdio.h>
 
26
 
 
27
using namespace std;
 
28
 
 
29
namespace gbxserialacfr {
 
30
namespace lockfile {
 
31
 
 
32
namespace {
 
33
 
 
34
    const char *LOCK_DIR = "/var/lock";
 
35
 
 
36
    // This function is inefficient, but avoids
 
37
    // all the weirdness of basename().
 
38
    std::string getBasename( const char *path )
 
39
    {
 
40
        char *pathCopy = strdup( path );
 
41
        char *base = basename( pathCopy );
 
42
        std::string baseString = base;
 
43
        free( pathCopy );
 
44
        return baseString;
 
45
    }
 
46
    
 
47
    void
 
48
    lock_dev(const char *dev, int lpid)
 
49
    {
 
50
        FILE *fd;
 
51
        char  pbuf[260], lbuf[260];
 
52
        int   ret;
 
53
 
 
54
        std::string devBase = getBasename( dev );
 
55
 
 
56
        sprintf(pbuf, "%s/PID..%d", LOCK_DIR, getpid());
 
57
        sprintf(lbuf, "%s/LCK..%s", LOCK_DIR, devBase.c_str());
 
58
 
 
59
        // Create a file with our PID
 
60
        fd = fopen(pbuf, "w");
 
61
        if (fd == 0)
 
62
        {
 
63
            stringstream ss;
 
64
            ss << "Failed to open file '"<<pbuf<<"' for writing: "<<strerror(errno);
 
65
            throw LockFileException( ss.str() );
 
66
        }
 
67
        fprintf(fd, "%10d\n", lpid);
 
68
        ret = fclose(fd);
 
69
        if ( ret != 0 )
 
70
        {
 
71
            stringstream ss; ss << "Problem closing lockfile '"<<pbuf<<"': " << strerror(errno);
 
72
            throw LockFileException( ss.str() );
 
73
        }
 
74
 
 
75
        // Create a file for the device
 
76
        if (link(pbuf, lbuf) < 0)
 
77
        {
 
78
            // A file already exists for the device...  Have a look at the PID
 
79
            fd = fopen(lbuf, "r");
 
80
            if ( fd == 0 )
 
81
            {
 
82
                unlink(pbuf);
 
83
                stringstream ss;
 
84
                ss << "Couldn't open file '"<<lbuf<<"' for reading: " << strerror(errno);
 
85
                throw LockFileException( ss.str() );
 
86
            }
 
87
 
 
88
            // Read the PID of the locker
 
89
            int pidOfLocker;
 
90
            ret = fscanf( fd, "%d", &pidOfLocker );
 
91
            fclose(fd);
 
92
 
 
93
            if ( ret != 1 )
 
94
            {
 
95
                unlink(pbuf);
 
96
                throw LockFileException( "Couldn't read PID of locker" );
 
97
            }
 
98
            if ( pidOfLocker <= 0 )
 
99
            {
 
100
                unlink(pbuf);
 
101
                stringstream ss; ss << "Invalid PID of locker: " << pidOfLocker;
 
102
                throw LockFileException( ss.str() );
 
103
            }
 
104
 
 
105
            if ( pidOfLocker == lpid )
 
106
            {
 
107
                // I'm the locker ?!?!
 
108
                unlink(pbuf);
 
109
                stringstream ss; ss << "device " << dev << " is already locked by me! (pid "<<lpid<<")";
 
110
                throw LockFileException( ss.str() );
 
111
            }
 
112
 
 
113
            // Look for the process which owns the lock
 
114
            if ( kill(pidOfLocker, 0) == 0 || errno == EPERM)
 
115
            {
 
116
                unlink(pbuf);
 
117
                stringstream ss; ss << "device " << dev << " is already locked by process PID " << pidOfLocker;
 
118
                throw LockFileException( ss.str() );
 
119
            }
 
120
            else if ( errno == ESRCH )
 
121
            {
 
122
                // The process which owns the lock no longer exists.  Clean up.
 
123
                ret = unlink( lbuf );
 
124
                if ( ret != 0 )
 
125
                {
 
126
                    unlink(pbuf);
 
127
                    stringstream ss; ss << "Couldn't unlink " << lbuf << ": " << strerror(errno);
 
128
                    throw LockFileException( ss.str() );
 
129
                }
 
130
 
 
131
                // Now create our own
 
132
                ret = link( pbuf, lbuf );
 
133
                if ( ret < 0 )
 
134
                {
 
135
                    unlink(pbuf);
 
136
                    stringstream ss; ss << "Couldn't link("<<pbuf<<","<<lbuf<<"): " << strerror(errno);
 
137
                    throw LockFileException( ss.str() );
 
138
                }
 
139
            }
 
140
            else
 
141
            {
 
142
                stringstream ss;
 
143
                ss << "lock_dev: Don't expect to see this errno after kill: " << strerror(errno);
 
144
                throw LockFileException( ss.str() );
 
145
            }
 
146
        }
 
147
 
 
148
        ret = unlink(pbuf);
 
149
        if ( ret < 0 )
 
150
        {
 
151
            stringstream ss;
 
152
            ss << "Couldn't unlink pbuf: " << strerror(errno);
 
153
            throw LockFileException( ss.str() );
 
154
        }
 
155
    }
 
156
 
 
157
    void
 
158
    unlock_dev(const char *dev, int lpid)
 
159
    {
 
160
        FILE *fd;
 
161
        char  lbuf[260];
 
162
        int   pidOfLocker = 0;
 
163
        char *p;
 
164
 
 
165
        if ((p = (char*)strrchr(dev, '/')))
 
166
            dev = p + 1;
 
167
        sprintf(lbuf, "%s/LCK..%s", LOCK_DIR, dev);
 
168
 
 
169
        fd = fopen(lbuf, "r");
 
170
        if (fd && fscanf(fd, "%d", &pidOfLocker) == 1 && pidOfLocker == lpid)
 
171
        {
 
172
            fclose(fd);
 
173
            unlink(lbuf);
 
174
        }
 
175
        else if (fd)
 
176
            fclose(fd);
 
177
    }
 
178
}
 
179
 
 
180
//////////////////////////////////////////////////////////////////////
 
181
 
 
182
LockFile::LockFile( const std::string &dev,
 
183
                    int lockPid )
 
184
    : dev_(dev),
 
185
      lockPid_(lockPid)
 
186
{
 
187
    lock_dev( dev_.c_str(), lockPid_ );
 
188
}
 
189
 
 
190
LockFile::~LockFile()
 
191
{
 
192
    // Don't allow exceptions from destructor
 
193
    try {
 
194
        unlock_dev( dev_.c_str(), lockPid_ );
 
195
    }
 
196
    catch ( ... ) {}
 
197
}
 
198
 
 
199
}
 
200
}