stagetime.cc

机器人仿真平台,和stage配合运行

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *                      
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: stagetime.cc,v 1.1 2002/08/13 01:54:49 gerkey Exp $
 *
 * this is the StageTime class, which gets current simulated time from
 * shared memory
 *
 */

#include "stagetime.h"
#include <stdio.h>
#include <sys/file.h>

// constuctor
StageTime::StageTime( stage_clock_t* clock, int fd ) 
{ 
  simtimep = &clock->time; 
  
  // use the first byte of the clock file to synchronize access
  InstallLock( fd, 0 );
}

void StageTime::Lock( void )
{
  // POSIX RECORD LOCKING METHOD
  struct flock cmd;

  cmd.l_type = F_WRLCK; // request write lock
  cmd.l_whence = SEEK_SET; // count bytes from start of file
  cmd.l_start = this->lock_byte; // lock my unique byte
  cmd.l_len = 1; // lock 1 byte

  fcntl( this->lock_fd, F_SETLKW, &cmd );
}

void StageTime::Unlock( void )
{
  // POSIX RECORD LOCKING METHOD
  struct flock cmd;

  cmd.l_type = F_UNLCK; // request  unlock
  cmd.l_whence = SEEK_SET; // count bytes from start of file
  cmd.l_start = this->lock_byte; // unlock my unique byte
  cmd.l_len = 1; // unlock 1 byte

  fcntl( this->lock_fd, F_SETLKW, &cmd );
}

int StageTime::GetTime(struct timeval* time)
{ 
  Lock();
  *time = *simtimep;
  Unlock();
  return(0);
}