Update of /cvsroot/playerstage/code/gazebo/libgazebo
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11622/libgazebo
Added Files:
Tag: ogre
Client.cc Iface.cc
Log Message:
Added new gazebo files
--- NEW FILE: Client.cc ---
/*
* Gazebo - Outdoor Multi-Robot Simulator
* Copyright (C) 2003
* Nate Koenig & 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
*
*/
/*
* Desc: Client object
* Author: Andrew Howard
* Date: 7 May 2003
* CVS: $Id: Client.cc,v 1.1.2.1 2006/12/16 22:41:14 natepak Exp $
*/
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/sem.h>
#include "gz_error.h"
#include "gazebo.h"
// Create a client object
Client::Client()
{
}
// Destroy a client
Client::~Client()
{
}
// Test for the presence of the server
int Client::Query(int serverId)
{
return this->SemQuery(serverId);
}
// Connect to the server
int Client::Connect(int serverId)
{
return this->ConnectWait(serverId, -1);
}
// Connect to the server
int Client::ConnectWait(int serverId, int clientId)
{
char *tmpdir;
char *user;
char filename[128];
// Check client id
if (clientId >= 0 && clientId >= 16)
{
GZ_ERROR1("invalid client ID [%d]", clientId);
return -1;
}
this->serverId = serverId;
this->clientId = clientId;
// Initialize semaphores
if (this->clientId >= 0)
if (this->SemInit() < 0)
return -1;
// Get the tmp dir
tmpdir = getenv("TMP");
if (!tmpdir)
tmpdir = "/tmp";
// Get the user
user = getenv("USER");
if (!user)
user = "nobody";
// Figure out the directory name
snprintf(filename, sizeof(filename), "%s/gazebo-%s-%d",
tmpdir, user, this->serverId);
assert(this->filename == NULL);
this->filename = strdup(filename);
GZ_MSG1(1, "opening %s", this->filename);
return 0;
}
// Disconnect from the server
int Client::Disconnect()
{
GZ_MSG1(1, "closing %s", this->filename);
assert(this->filename != NULL);
free(this->filename);
// Finalize semaphores
if (this->clientId >= 0)
if (this->SemFini() < 0)
return -1;
return 0;
}
// Wait for new data to be posted (blocking)
int Client::Wait()
{
if (this->clientId >= 0)
return this->SemWait();
return 0;
}
// Initialize semaphores
int Client::SemQuery(int serverId)
{
int semKey;
semKey = GZ_SEM_KEY + serverId;
// Try to get the semaphore
if (semget(semKey, 0, S_IRWXU) < 0)
{
// No semaphore, so no server
if (errno == ENOENT)
return 1;
// Ooops, some kind of error
GZ_ERROR1("failed to query semaphore [%s]", strerror(errno));
return -1;
}
// We have a server
return 0;
}
// Initialize semaphores
int Client::SemInit()
{
this->semKey = GZ_SEM_KEY + this->serverId;
// Get the client semaphore group
this->semId = semget(this->semKey, 0, S_IRWXU);
if (this->semId < 0)
{
GZ_ERROR1("Failed to allocate semaphore [%s]", strerror(errno));
GZ_ERROR("The server does not appear to be running");
return -1;
}
return 0;
}
// Finalize semaphores
int Client::SemFini()
{
return 0;
}
// Wait for new data to be posted (blocking)
int Client::SemWait()
{
struct sembuf operations[1];
operations[0].sem_num = this->clientId;
operations[0].sem_op = -1;
operations[0].sem_flg = SEM_UNDO;
if (semop(this->semId, operations, 1) < 0)
{
GZ_ERROR1("error on semaphore wait [%s]", strerror(errno));
return -1;
}
return 0;
}
--- NEW FILE: Iface.cc ---
/*
* Gazebo - Outdoor Multi-Robot Simulator
* Copyright (C) 2003
* Nate Koenig & 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
*
*/
/*
* Desc: Generic interface support
* Author: Andrew Howard
* Date: 21 Apr 2003
* CVS: $Id: Iface.cc,v 1.1.2.1 2006/12/16 22:41:14 natepak Exp $
*/
#include <assert.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include "gazebo.h"
#include "gz_error.h"
//////////////////////////////////////////////////////////////////////////////
// Create an interface
Iface::Iface(const std::string &type)
{
this->type = type;
this->filename = NULL;
}
//////////////////////////////////////////////////////////////////////////////
// Destroy an interface
Iface::~Iface()
{
delete this->filename;
}
//////////////////////////////////////////////////////////////////////////////
// Work out the filename
const char *Iface::Filename(const char *id)
{
char filename[128];
if (this->server != NULL)
{
snprintf(filename, sizeof(filename), "%s/%s.%s",
this->server->filename, this->type.c_str(), id);
}
else if (this->client != NULL)
{
snprintf(filename, sizeof(filename), "%s/%s.%s",
this->client->filename, this->type.c_str(), id);
}
else
assert(0);
if (this->filename)
free(this->filename);
this->filename = strdup(filename);
return this->filename;
}
//////////////////////////////////////////////////////////////////////////////
// Create an interface (server)
int Iface::Create(Server *server, const char *id)
{
this->server = server;
// Went cant have null id's
if (id == NULL)
{
GZ_ERROR1("interface [%s] id is NULL", this->type.c_str());
return -1;
}
// We cannot have id with '.'
if (strchr(id, '.'))
{
GZ_ERROR1("invalid id [%s] (must not contain '.')", id);
return -1;
}
// Work out the filename
this->Filename(id);
// Create and open the file
this->mmapFd = open(this->filename, O_RDWR | O_CREAT | O_TRUNC, S_IREAD |
S_IWRITE);
if (this->mmapFd < 0)
{
GZ_ERROR1("error creating mmap file: %s", strerror(errno));
return -1;
}
// Set the file to the correct size
if (ftruncate(this->mmapFd, sizeof(*this)) < 0)
{
GZ_ERROR1("error setting size of mmap file: %s", strerror(errno));
return -1;
}
// Map the file into memory
this->mMap = mmap(0, sizeof(*this), PROT_READ | PROT_WRITE, MAP_SHARED,
this->mmapFd, 0);
if (this->mMap == MAP_FAILED)
{
GZ_ERROR1("error mapping mmap file: %s", strerror(errno));
return -1;
}
memset(this->mMap, 0, sizeof(*this));
((Iface*) this->mMap)->version = LIBGAZEBO_VERSION;
((Iface*) this->mMap)->size = sizeof(*this);
// Print the name, version info
GZ_MSG3(5, "creating %s %03X %d", this->filename,
((Iface*) this->mMap)->version,
((Iface*) this->mMap)->size);
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Create the interface
int Iface::Create(Server *server, const char *id,
const std::string &modelType, int modelId,
int parentModelId)
{
if (this->Create(server,id) != 0)
return -1;
this->modelType = modelType;
this->modelId = modelId;
this->parentModelId = parentModelId;
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Destroy the interface (server)
int Iface::Destroy()
{
// Unmap the file
munmap(this->mMap, sizeof(Iface));
this->mMap = NULL;
// Close the file
close(this->mmapFd);
// Delete the file
GZ_MSG1(5, "deleting %s", this->filename);
if (unlink(this->filename))
{
GZ_ERROR1("error deleting mmap file: %s", strerror(errno));
return -1;
}
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Open an existing interface (client)
int Iface::Open(Client *client, const char *id)
{
this->client = client;
// Work out the filename
this->Filename(id);
// Open the mmap file
this->mmapFd = open(this->filename, O_RDWR);
if (this->mmapFd <= 0)
{
GZ_ERROR2("error opening device file %s : %s", this->filename,
strerror(errno));
return -1;
}
// Map the mmap file
this->mMap = mmap(0, sizeof(*this), PROT_READ | PROT_WRITE, MAP_SHARED,
this->mmapFd, 0);
if (this->mMap == MAP_FAILED)
{
GZ_ERROR1("error mapping device file: %s", strerror(errno));
return -1;
}
// Make sure everything is consistent
if (((Iface*) this->mMap)->size < sizeof(*this))
{
GZ_ERROR2("expected file size: %d < %d", ((Iface*) this->mMap)->size,
sizeof(*this));
return -1;
}
// Print the name, version info
GZ_MSG3(5, "opening %s %03X %d", this->filename,
((Iface*) this->mMap)->version,
((Iface*) this->mMap)->size);
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Close the interface (client)
int Iface::Close()
{
// Unmap the file
munmap(this->mMap, sizeof(Iface));
this->mMap = NULL;
// Close the file
GZ_MSG1(5, "closing %s", this->filename);
close(this->mmapFd);
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Lock the interface.
int Iface::Lock(int blocking)
{
// Some 2.4 kernels seem to screw up the lock count somehow; keep an eye out
//printf(" lock %p %s\n", this, this->filename);
// Lock the file
if (flock(this->mmapFd, LOCK_EX) != 0)
{
GZ_ERROR2("flock %s error: %s", this->filename, strerror(errno));
return -1;
}
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Unlock the interface
void Iface::Unlock()
{
//printf("unlock %p %s\n", this, this->filename);
// Unlock the file
if (flock(this->mmapFd, LOCK_UN) != 0)
{
GZ_ERROR1("flock error: %s", strerror(errno));
return;
}
return;
}
//////////////////////////////////////////////////////////////////////////////
// Tell clients that new data is available
int Iface::Post()
{
assert(this->server);
return this->server->Post();
}
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys - and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
