arcnet_sensors.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

 /*********************************************************
 *
 * This source code is part of the Carnegie Mellon Robot
 * Navigation Toolkit (CARMEN)
 *
 * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas
 * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss,
 * and Jared Glover
 *
 * CARMEN 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.
 *
 * CARMEN 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 CARMEN; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

#include <assert.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <ctype.h>
#include <carmen/carmen.h>
#include "extend.h"
#include "Nclient.h"
#include "setup.h"

extern char *robd_program_name; 

void ARGCV_Create(char *string, int *argc_ret, char ***argv_ret)
{
  unsigned char literal_quote;
  unsigned char literal_slash;
  unsigned char between_args;
  long unsigned int length;
  long unsigned int si;
  long unsigned int vi;
  char *vstring;
  int argc;
  char **argv;
  
  literal_quote = 0;
  literal_slash = 0;
  between_args = 1;
  while ((*string != '\0') && (isspace(*string)))
    string++;

  length = strlen(string);
  if (length == 0) {
    *argc_ret = 0;
    *argv_ret = NULL;
    return;
  }
  
  vstring = (char *) calloc(length + 1, 1);
  carmen_test_alloc(vstring);
  argc = 0;
  
  argv = (char **) calloc(sizeof(char *), 1);
  carmen_test_alloc(argv);

  vi = 0;

  for (si = 0; si < length; si++) {
    if ((literal_quote == 0) && (literal_slash == 0) &&
	(string[si] == '"')) {
      literal_quote = 1;
      if (between_args == 1) {
	between_args = 0;
	argv = (char **) realloc(argv, (argc + 1) * sizeof(char *));
	carmen_test_alloc(argv);
	argv[argc] = vstring + vi;
	argc++;
      }
    } else if ((literal_quote == 1) && (literal_slash == 0) &&
	       (string[si] == '"')) {
      literal_quote = 0;
    } else if ((literal_slash == 0) && (string[si] == '\\')) {
      literal_slash = 1;
      if (between_args == 1) {
	between_args = 0;
	argv = (char **) realloc(argv, (argc + 1) * sizeof(char *));
	carmen_test_alloc(argv);
	argv[argc] = vstring + vi;
	argc++;
      }
    } else {
      if ((literal_quote == 0) && (literal_slash == 0)) {
	if (string[si] == 0x0a) {
	  if (between_args == 0) {
	    between_args = 1;
	    vstring[vi] = '\0';
	  }
	  vstring[vi] = '\0';
	  argv = (char **) realloc(argv, (argc + 1) * sizeof(char *));
	  carmen_test_alloc(argv);
	  argv[argc] = vstring + vi;
	  vi++;
	  argc++;
	  continue;
	} 
	if (isspace(string[si])) {     
	  if (between_args == 0) {
	    between_args = 1;
	    vstring[vi] = '\0';
	    vi++;
	  }
	  continue;
	}
      }       
      if (between_args == 1) {
	between_args = 0;
	argv = (char **) realloc(argv, (argc + 1) * sizeof(char *));
	carmen_test_alloc(argv);
	argv[argc] = vstring + vi;
	argc++;
      } 
      vstring[vi] = string[si];
      vi++;
      literal_slash = 0;
    } 
  }
  vstring[vi] = '\0';
  *argc_ret = argc;
  *argv_ret = argv;  
}

void ARGCV_Free(int argc __attribute__ ((unused)), char **argv)
{
  free(*argv);
  free(argv);
}

long int list_fill(double *mat, ...) 
{
  va_list ap; 
  long int r; 
  long int c; 
  long int i; 

  va_start(ap, mat);
  r = va_arg(ap, int);
  c = va_arg(ap, int);
  for (i = 0; i < r * c; i++) {
    (*(mat++)) = va_arg(ap, double);
  }
  va_end(ap);
  return 0;
}

static
void ParseSonarCommand(struct N_XSonar *xsonar, char *command, char *base_id)
{
  char **argv; 
  int argc; 
  double x, y, sine, cosine;

  ARGCV_Create(command, &argc, &argv);
  
  if(argc < 7)
    return;

  xsonar->ID = (char *)calloc(strlen(base_id) + strlen(argv[1]) + 2, 1);
  carmen_test_alloc(xsonar->ID);
  sprintf(xsonar->ID, "%s:%s", base_id, argv[1]);
  
  xsonar->Reference = strdup(argv[6]);
  
  x = atof(argv[2]);
  y = atof(argv[3]);
  cosine = atof(argv[4]);  
  sine = atof(argv[5]);
  
  xsonar->Configuration = (double *)calloc(9, sizeof(double));
  carmen_test_alloc(xsonar->Configuration);
  list_fill(xsonar->Configuration, 3, 3, 
	    cosine, -sine, x, sine, cosine, y, 0.0, 0.0, 1.0);
  
  ARGCV_Free(argc, argv);
  
  return;
}

void SON_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
  unsigned short s; 
  unsigned short sn; 
  char *const_string; 
  char **set_argv; 
  char **son_argv; 
  int set_argc; 
  int son_argc; 
  struct N_SonarSet *sonar_set; 
  struct N_XSonarSet *xset; 

  const_string = SETUP_GetValue("[sonar]sonar_sets");
  
  assert(const_string != ((void *)0));
  
  ARGCV_Create(const_string, &set_argc, &set_argv);

  if (set_argc > N_MAX_SONAR_SET_COUNT) {
    fprintf(stderr, "Setup: too many sonar sets in %s.\n"
	    "  Using the first %d.\n", "[sonar]sonar_sets", 
	    N_MAX_SONAR_SET_COUNT);
    rs->SonarController.SonarSetCount = N_MAX_SONAR_SET_COUNT;
  } else {
    rs->SonarController.SonarSetCount = set_argc;
  } 

  for (s = 0; s < rs->SonarController.SonarSetCount; s++) {
    sonar_set = &(rs->SonarController.SonarSet[s]);
    xset = &(rs_ext->SonarController.SonarSet[s]);

    const_string = SETUP_ExtGetValue(set_argv[s], "main_lobe");
    xset->MainLobe = (const_string != NULL) ? atof(const_string) : 0.0;
    const_string = SETUP_ExtGetValue(set_argv[s], "blind_lobe");
    xset->BlindLobe = (const_string != NULL) ? atof(const_string) : 0.0;
    const_string = SETUP_ExtGetValue(set_argv[s], "side_lobe");
    xset->SideLobe = (const_string != NULL) ? atof(const_string) : 0.0;
    const_string = SETUP_ExtGetValue(set_argv[s], "range");
    xset->Range = (const_string != NULL) ? atof(const_string) : 0.0;
    const_string = SETUP_ExtGetValue(set_argv[s], "blind_lobe_attenuation");
    xset->BlindLobeAttenuation = 
      (const_string != NULL) ? atof(const_string) : 0.0;
    const_string = SETUP_ExtGetValue(set_argv[s], "side_lobe_attenuation");
    xset->SideLobeAttenuation = 
      (const_string != NULL) ? atof(const_string) : 0.0;
    
    const_string = SETUP_ExtGetValue(set_argv[s], "sonars");
    
    if (const_string == NULL) {
      sonar_set->SonarCount = 0;
    } else { 
      ARGCV_Create(const_string, &son_argc, &son_argv);
      
      if (son_argc > N_MAX_SONAR_COUNT) {
	printf("Warning: too many sonars in [%s]%s.\n"
		"  Using the first %d.\n", set_argv[s], "sonars",
		N_MAX_SONAR_COUNT);
	sonar_set->SonarCount = N_MAX_SONAR_COUNT;
      } else {
	sonar_set->SonarCount = son_argc;
      }
      
      for (sn = 0; sn < sonar_set->SonarCount; sn++) {
	const_string = SETUP_ExtGetValue(set_argv[s], son_argv[sn]);
	if (const_string == NULL) {
	  fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
		  set_argv[s], son_argv[sn]);
	} else {
	  ParseSonarCommand(&(xset->Sonar[sn]), const_string, set_argv[s]);
	}
      }
      ARGCV_Free(son_argc, son_argv);
    }
  } 
  ARGCV_Free(set_argc, set_argv);
  return;
}

void INIT_InitializeSonars(long int robot_id)
{
  unsigned char active; 
  char set_key[20]; 
  char *forder_str; 
  char *end; 
  char **argv; 
  unsigned int set; 
  unsigned int sonar_num; 
  unsigned int sonar_index; 
  int argc; 
  struct N_RobotState *rs; 
  struct N_SonarSet *sonar_set; 

  rs = N_GetRobotState(robot_id);
  if (rs == NULL)
    return ;
  
  if (N_GetSonarConfiguration(robot_id))
    return ;
  
  active = 0;
  for (set = 0; set < rs->SonarController.SonarSetCount; set++) {
    sonar_set = &(rs->SonarController.SonarSet[set]);
    sprintf(set_key, "sonset%u", set);
    forder_str = SETUP_ExtGetValue(set_key, "firing_order");
    if (forder_str != NULL) {
      ARGCV_Create(forder_str, &argc, &argv);
      active = 1;
      for (sonar_index = 0; sonar_index < sonar_set->SonarCount; 
	   sonar_index++) {
	sonar_num = strtol(argv[sonar_index], &end, 0);
	if (end == argv[sonar_index]) {
	  fprintf(stderr, 
		  "%s: (warning) sonar entry %s in \"[%s]%s\" is invalid.\n",
		  robd_program_name, argv[sonar_index], set_key,
		  "firing_order");
	  break;
	} 
	if (sonar_num == 255) {
	  break;
	}