gz_server.c

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  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: gz_server.c,v 1.7 2004/11/15 22:19:45 inspectorg 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"


// Local functions
static int gz_server_sem_init(gz_server_t *self, int force);
static int gz_server_sem_fini(gz_server_t *self);
static int gz_server_sem_post(gz_server_t *self);


// Create a server object
gz_server_t *gz_server_alloc()
{
  gz_server_t *self;

  self = calloc(1, sizeof(gz_server_t));

  return self;
}


// Destroy a server
void gz_server_free(gz_server_t *self)
{
  free(self);
  return;
}


// Initialize the server
int gz_server_init(gz_server_t *self, int server_id, int force)
{
  char *tmpdir;
  char *user;
  char filename[128];

  self->server_id = server_id;

  // Initialize semaphores.  Do this first to make sure we dont have
  // another server running with the same id.
  if (gz_server_sem_init(self, 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, self->server_id);
  assert(self->filename == NULL);
  self->filename = strdup(filename);  

  GZ_MSG1(5, "creating %s", self->filename);
  
  // Create the directory
  if (mkdir(self->filename, S_IRUSR | S_IWUSR | S_IXUSR) != 0)
  {
    if (errno == EEXIST)
    {
      GZ_ERROR1("directory [%s] already exists (previous crash?)", self->filename);
      GZ_ERROR("remove the directory and re-run gazebo");
      return -1;
    }
    else
    {    
      GZ_ERROR2("failed to create [%s] : [%s]", self->filename, strerror(errno));
      return -1;
    }
  }
  
  return 0;
}


// Finialize the server
int gz_server_fini(gz_server_t *self)
{
  char cmd[1024];
  
  GZ_MSG1(5, "deleting %s", self->filename);
  
  // Delete the server dir
  if (rmdir(self->filename) != 0)
  {
    GZ_MSG2(0, "failed to cleanly remove [%s] : [%s]", self->filename, strerror(errno));
    snprintf(cmd, sizeof(cmd), "rm -rf %s", self->filename);
    system(cmd);
  }
  
  assert(self->filename != NULL);
  free(self->filename);

  // Finalize semaphores
  if (gz_server_sem_fini(self) < 0)
    return -1;

  return 0;
}


// Tell clients that new data is available
int gz_server_post(gz_server_t *self)
{
  return gz_server_sem_post(self);
}


// Initialize semaphores
int gz_server_sem_init(gz_server_t *self, int force)
{
  int i;
  union semun arg;
  unsigned short values[16];

  self->sem_key = GZ_SEM_KEY + self->server_id;

  // If force is set, use the semaphore regardless of who else
  // might currently be using it
  if (force)
    self->sem_cid = semget(self->sem_key, 16, IPC_CREAT | S_IRWXU);
  else
    self->sem_cid = semget(self->sem_key, 16, IPC_CREAT | IPC_EXCL | S_IRWXU);
  
  // Create semaphores for clients
  if (self->sem_cid < 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(self->sem_cid, 0, SETALL, arg) < 0)
  {
    GZ_ERROR1("failed to initialize semaphore [%s]", strerror(errno));
    return -1;
  }
  
  return 0;
}


// Finalize semaphores
int gz_server_sem_fini(gz_server_t *self)
{  
  union semun arg;

  if (semctl(self->sem_cid, 0, IPC_RMID, arg) < 0)
  {
    GZ_ERROR1("failed to deallocate semaphore [%s]", strerror(errno));
    return -1;
  }
  return 0;
}


// Release waiting clients
int gz_server_sem_post(gz_server_t *self)
{
  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(self->sem_cid, 0, SETALL, arg) < 0)
  {
    GZ_ERROR1("failed to initialize semaphore [%s]", strerror(errno));
    return -1;
  }

  /* REMOVE
  struct sembuf operations[1];
  
  operations[0].sem_num = 0;
  operations[0].sem_op = 1;
  operations[0].sem_flg = SEM_UNDO;
  
  if (semop(self->semid, operations, 1) < 0)
  {
    GZ_ERROR1("error on semaphore post [%s]", strerror(errno));
    return -1;
  }
  */
  
  return 0;
}