Update of /cvsroot/playerstage/code/gazebo/libgazebo
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv12461/libgazebo
Added Files:
Tag: ogre
SConscript Server.cc gazebo.h gz_error.c gz_error.h
Log Message:
Added new gazebo files
--- NEW FILE: gz_error.h ---
/*
* 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: Error handling
* Author: Andrew Howard
* Date: 21 Apr 2003
* CVS: $Id: gz_error.h,v 1.6.2.2 2006/12/16 22:43:22 natepak Exp $
*/
#ifndef GZ_ERROR_H
#define GZ_ERROR_H
#include <stdio.h>
/** @brief @internal Function for print and logging errors. Do not
call this function directly; use the macros below.
*/
void gz_error_print(const char *type, int level, const char *file, int line,
const char *fmt, ...);
/** Error macros */
#define GZ_ERROR(m) \
gz_error_print("error", 0, __FILE__, __LINE__, m)
#define GZ_ERROR1(m, a) \
gz_error_print("error", 0, __FILE__, __LINE__, m, a)
#define GZ_ERROR2(m, a, b) \
gz_error_print("error", 0, __FILE__, __LINE__, m, a, b)
/** Diagnostic macros */
#define GZ_MSG(level, m) \
gz_error_print("msg", level, __FILE__, __LINE__, m)
#define GZ_MSG1(level, m, a)\
gz_error_print("msg", level, __FILE__, __LINE__, m, a)
#define GZ_MSG2(level, m, a, b)\
gz_error_print("msg", level, __FILE__, __LINE__, m, a, b)
#define GZ_MSG3(level, m, a, b, c)\
gz_error_print("msg", level, __FILE__, __LINE__, m, a, b, c)
#endif
--- NEW FILE: gazebo.h ---
/*
* 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: External interfaces for Gazebo
* Author: Andrew Howard
* Date: 6 Apr 2003
* CVS: $Id: gazebo.h,v 1.84.2.3 2006/12/16 22:43:22 natepak Exp $
*/
#ifndef GAZEBO_H
#define GAZEBO_H
#include <string>
#include <sys/types.h>
#include <stdlib.h>
#include <stdint.h>
/** \defgroup libgazebo libgazebo
See \ref libgazebo_usage for information on using libgazebo.
*/
/***************************************************************************
* Constants, etc
**************************************************************************/
//! \addtogroup libgazebo
//! \{
//! Interface version number
#define LIBGAZEBO_VERSION 0x070
//! \}
//! \addtogroup libgazebo
//! \{
//! Pose class
class Pose
{
//! Position information
public: double x, y, z;
//! Rotation information. Euler angles
public: double roll, pitch, yaw;
};
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \defgroup utility Error-handling
\{
*/
/***************************************************************************/
/** \brief \internal Initialize error logging
\param print Set to 0 to stop messages being printed to stderr.
\param level Debug level: 0 = important messages, 1 = useful
messages, 2+ = all messages.
*/
void gz_error_init(int print, int level);
/** Retrieve the last error (as a descriptive string). Most functions
in will return 0 on success and non-zero value on error; a
descriptive error message can be obtained by calling this
function. */
const char *gz_error_str(void);
/** \}*/
//! \}
/***************************************************************************/
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \defgroup server Server object
The server object is used by the Gazebo server to establish and
maintain connections with clients.
\internal
\{
*/
//! \brief Server class
class Server
{
//! \brief Constructor
public: Server();
//! \brief Destructor
public: virtual ~Server();
//! \brief Initialize the server
public: int Init(int serverId, int force);
//! \brief Finalize the server
public: int Fini();
//! \brief Tell clients that new data is available
public: int Post();
private: int SemInit(int force);
private: int SemFini();
private: int SemPost();
//! The server id
public: int serverId;
//! The directory containing mmap files
public: char *filename;
//! The semphore key and id
public: int semKey, semId;
};
/** \}*/
//! \}
/***************************************************************************/
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \defgroup client Client object
The client object is used by Gazebo clients to establish a connection
with a running server. See the \ref libgazebo_usage for an overview.
\{
*/
//! \brief Semaphore key used by Gazebo
#define GZ_SEM_KEY 0x135135FA
//! \brief Reserved client IDs.
//!
//! User programs may use numbers in the range GZ_SEM_NUM_USER to
//! GZ_SEM_NUM_USER_LAST, inclusive. All other semaphore numbers are
//! reserved.
#define GZ_CLIENT_ID_USER_FIRST 0x00
#define GZ_CLIENT_ID_USER_LAST 0x07
#define GZ_CLIENT_ID_WXGAZEBO 0x08
#define GZ_CLIENT_ID_PLAYER 0x09
//! Client class
class Client
{
//! Create a new client
public: Client();
//! Destroy a client
public: virtual ~Client();
//! Test for the presence of the server.
//! \returns The return value is 0 if the server is present; +1 if
//! the server is not present; -1 if there is an error.
public: int Query(int server_id);
//! Connect to the server (non-blocking mode).
public: int Connect(int server_id);
//! \brief Connect to the server (blocking mode).
//! \param server_id Server ID; each server must have a unique id.
//! \param client_id Client ID; in blocking mode, each client must have a
unique id.
public: int ConnectWait(int server_id, int client_id);
//! Disconnect from the server
public: int Disconnect();
//! \brief Wait for new data to be posted (blocking mode).
//! \returns Returns 0 on success, -1 on error.
public: int Wait();
private: int SemQuery(int server_id);
private: int SemInit();
private: int SemFini();
private: int SemWait();
//! The server id
public: int serverId;
//! The client id
public: int clientId;
//! The directory containing mmap files
public: char *filename;
//! The semphore key and id
public: int semKey, semId;
};
/** \} */
//! \}
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \internal \defgroup iface Common interface structures
All interfaces share this common structure.
\{
*/
//! Max length of model type string
#define GAZEBO_MAX_MODEL_TYPE 128
class Iface
{
//! \brief Create an interface
public: Iface(const std::string &type);
//! \brief Destroy an interface
public: virtual ~Iface();
//! \brief Create the interface (used by Gazebo server)
public: int Create(Server *server, const char *id);
//! \brief Create the interface (used by Gazebo server)
public: int Create(Server *server, const char *id,
const std::string &modelType, int modelId,
int parentModelId);
//! \brief Destroy the interface (server)
public: int Destroy();
//! Open an existing interface
public: int Open(Client *client, const char *id);
//! Close the interface
public: int Close();
//! Lock the interface. Set blocking to 1 if the caller should block
//! until the lock is acquired. Returns 0 if the lock is acquired.
public: int Lock(int blocking);
//! Unlock the interface
public: void Unlock();
//! Tell clients that new data is available
public: int Post();
private: const char *Filename(const char *id);
//! The server we are associated with
public: Server *server;
//! The client we are associated with
public: Client *client;
//! File descriptor for the mmap file
public: int mmapFd;
//! Pointer to the mmap'ed mem
public: void *mMap;
//! The name of the file we created/opened
public: char *filename;
//! Interface version number
public: int version;
//! Allocation size
public: size_t size;
//! Type of model that owns this interface
//public: char modelType[GAZEBO_MAX_MODEL_TYPE];
public: std::string modelType;
//! ID of the model that owns this interface
public: int modelId;
//! ID of the parent model
public: int parentModelId;
protected: std::string type;
};
/** \} */
//! \}
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \defgroup simulator simulator
The simulator interface provides access to certain global properties
of the server, such as the current simulation time-step.
\{
*/
//! Common simulator data
class SimIface : public Iface
{
//! Create an interface
public: SimIface():Iface("sim") {}
//! Destroy an interface
public: virtual ~SimIface() {}
//! Elapsed simulator time
public: double sim_time;
//! Accumpated pause time (this interface may be updated with the
//! server is paused).
public: double pause_time;
// Elapsed real time since start of simulation (from system clock).
public: double real_time;
//! Pause simulation (set by client)
public: int pause;
//! Reset simulation (set by client)
public: int reset;
//! Save current poses to world file (set by client)
public: int save;
};
/** \} */
//! \}
/***************************************************************************/
//! \addtogroup libgazebo
//! \{
/** \defgroup camera camera
The camera interface allows clients to read images from a simulated
camera. This interface gives the view of the world as the camera
would see it.
Images are in packed RGB888 format.
\{
*/
//! Maximum image pixels (width x height)
#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
//! The camera interface
class CameraIface : public Iface
{
public: CameraIface():Iface("camera") {}
public: virtual ~CameraIface() {}
//! Data timestamp
public: double time;
//! Image dimensions (in pixels)
public: unsigned int width, height;
//! Image pixel data
public: unsigned int image_size;
public: unsigned char image[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
};
/** \} */
//! \}
/***************************************************************************/
/// @addtogroup libgazebo
/// @{
/** @defgroup position position
The position interface allows clients to send commands to and read
odometric data from simulated mobile robot bases, such as the
Pioneer2AT or ATRV Jr. This interface handles both 2D and 3D data.
@{
*/
/// Position interface
class PositionIface : public Iface
{
//! Constructor
public: PositionIface():Iface("position") {}
//! Destructor
public: virtual ~PositionIface() {}
//! Data timestamp
public: double time;
//! Pose (usually global cs)
public: Pose pose;
//! Velocity
public: Pose velocity;
//! Motor stall flag
public: int stall;
//! Enable the motors
public: int cmd_enable_motors;
//! Commanded robot velocities (robot cs)
public: Pose cmdVelocity;
};
/** @} */
/// @}
#endif
--- NEW FILE: SConscript ---
#Import variable
Import('env prefix')
sources = Split('Server.cc Client.cc Iface.cc gz_error.c')
sharedLib = env.SharedLibrary('gazebo', sources)
staticLib = env.StaticLibrary('gazebo', sources)
env.Install(prefix+'/lib', sharedLib)
env.Install(prefix+'/lib', staticLib)
--- NEW FILE: gz_error.c ---
/*
* 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: Error handling
* Author: Andrew Howard
* Date: 13 May 2002
* CVS: $Id: gz_error.c,v 1.6.2.2 2006/12/16 22:43:22 natepak Exp $
**************************************************************************/
#include <stdarg.h>
#include <string.h>
#include "gazebo.h"
#include "gz_error.h"
// Print to stdout?
static int error_print = 1;
// User-selected msg level: 0 for the most important messages (always
// printed); 9 for the least important.
static int error_level = 9;
// Last error
static char error_str[1024];
// Initialize error logging
void gz_error_init(int print, int level)
{
error_print = print;
error_level = level;
return;
}
// Get the most recent error.
const char *gz_error_str(void)
{
return error_str;
}
// Function for printing and logging errors.
void gz_error_print(const char *type, int level, const char *file, int line,
const char *fmt, ...)
{
va_list term_va, str_va;
va_start(term_va, fmt);
va_copy(str_va, term_va);
if (strcmp(type, "error") == 0)
{
if (error_print)
{
fprintf(stderr, "%s:%d ", file, line);
vfprintf(stderr, fmt, term_va);
fprintf(stderr, "\n");
}
vsnprintf(error_str, sizeof(error_str), fmt, str_va);
}
else
{
if (level <= error_level)
{
if (error_print)
{
fprintf(stderr, "%s:%d ", file, line);
vfprintf(stderr, fmt, term_va);
fprintf(stderr, "\n");
}
}
vsnprintf(error_str, sizeof(error_str), fmt, str_va);
}
va_end(term_va);
va_end(str_va);
return;
}
--- NEW FILE: Server.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: Server object
* Author: Andrew Howard
* Date: 7 May 2003
* CVS: $Id: Server.cc,v 1.1.2.1 2006/12/16 22:43:22 natepak Exp $
*/
#if HAVE_CONFIG_H
#include <config.h>
#endif
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/sem.h>
#include "replace.h"
#include "gz_error.h"
#include "gazebo.h"
// Create a server object
Server::Server()
{
this->filename = NULL;
}
// Destroy a server
Server::~Server()
{
return;
}
// Initialize the server
int Server::Init(int serverId, int force)
{
char *tmpdir;
char *user;
char filename[128];
this->serverId = serverId;
// Initialize semaphores. Do this first to make sure we dont have
// another server running with the same id.
if (this->SemInit(force) < 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(5, "creating %s", this->filename);
// Create the directory
if (mkdir(this->filename, S_IRUSR | S_IWUSR | S_IXUSR) != 0)
{
if (errno == EEXIST)
{
GZ_ERROR1("directory [%s] already exists (previous crash?)",
this->filename);
GZ_ERROR("remove the directory and re-run gazebo");
return -1;
}
else
{
GZ_ERROR2("failed to create [%s] : [%s]", this->filename,
strerror(errno));
return -1;
}
}
return 0;
}
// Finialize the server
int Server::Fini()
{
char cmd[1024];
GZ_MSG1(5, "deleting %s", this->filename);
// Delete the server dir
if (rmdir(this->filename) != 0)
{
GZ_MSG2(0, "failed to cleanly remove [%s] : [%s]", this->filename,
strerror(errno));
snprintf(cmd, sizeof(cmd), "rm -rf %s", this->filename);
system(cmd);
}
assert(this->filename != NULL);
free(this->filename);
// Finalize semaphores
if (this->SemFini() < 0)
return -1;
return 0;
}
// Tell clients that new data is available
int Server::Post()
{
return this->SemPost();
}
// Initialize semaphores
int Server::SemInit(int force)
{
int i;
union semun arg;
unsigned short values[16];
this->semKey = GZ_SEM_KEY + this->serverId;
// If force is set, use the semaphore regardless of who else
// might currently be using it
if (force)
this->semId = semget(this->semKey, 16, IPC_CREAT | S_IRWXU);
else
this->semId = semget(this->semKey, 16, IPC_CREAT | IPC_EXCL | S_IRWXU);
// Create semaphores for clients
if (this->semId < 0)
{
GZ_ERROR1("Failed to allocate semaphore [%s]", strerror(errno));
if (errno == EEXIST)
{
GZ_ERROR("There appears to be another server running.");
GZ_ERROR("Use the -s flag to try a different server id,");
GZ_ERROR("or use the -f flag if you definitely want to use this id.");
}
return -1;
}
// Set initial semaphore values
for (i = 0; i < 16; i++)
values[i] = 0;
arg.array = values;
if (semctl(this->semId, 0, SETALL, arg) < 0)
{
GZ_ERROR1("failed to initialize semaphore [%s]", strerror(errno));
return -1;
}
return 0;
}
// Finalize semaphores
int Server::SemFini()
{
union semun arg;
if (semctl(this->semId, 0, IPC_RMID, arg) < 0)
{
GZ_ERROR1("failed to deallocate semaphore [%s]", strerror(errno));
return -1;
}
return 0;
}
// Release waiting clients
int Server::SemPost()
{
int i;
union semun arg;
unsigned short values[16];
// Release all waiting clients
for (i = 0; i < 16; i++)
values[i] = 1;
arg.array = values;
if (semctl(this->semId, 0, SETALL, arg) < 0)
{
GZ_ERROR1("failed to initialize semaphore [%s]", strerror(errno));
return -1;
}
return 0;
}
-------------------------------------------------------------------------
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
