arcnet_sensors.c

卡内基梅隆大学（CMU）开发的移动机器人控制开发软件包。可对多种机器人进行控制

	if (sonar_num >= sonar_set->SonarCount) {
	  fprintf(stderr, 
		  "%s: (warning) invalid sonar number %s in \"[%s]%s\".\n",
		  robd_program_name, argv[sonar_index], set_key, 
		  "firing_order");
	  break;
	}
	
	sonar_set->FiringOrder[sonar_index] = sonar_num;
      } 

      ARGCV_Free(argc, argv);
      
      sonar_set->FiringOrder[sonar_index] = 255;
    } 
  }
  
  if (active) {
    N_SetSonarConfiguration(robot_id);
  }
  return ;
}

void mysincos(double angle, double *sine, double *cosine) 
{
  *sine = sin(angle);
  *cosine = cos(angle);
  return;
}

static int ParseBumperCommand(struct N_XBumperSet *xset, 
			      unsigned short *bindex, char *set_id, 
			      char *command)
{
  char *id = NULL; 
  char *id_start = NULL; 
  char *reference = NULL; 
  char *tmp; 
  char **argv; 
  unsigned short n = 0; 
  unsigned short i; 
  int argc; 
  double sine; 
  double cosine; 
  double x = 0.0; 
  double y = 0.0; 
  double start_angle = 0.0; 
  double sweep = 0.0; 

  ARGCV_Create(command, &argc, &argv);
  
  if (argc <= 0) 
    return -1;
  
  if (strcasecmp(argv[0], "CreateBumperArc") == 0) {
    if (argc < 8) {
      ARGCV_Free(argc, argv);
      return -1;
    } 

    if ((n = atoi(argv[1])) == 0) {
      ARGCV_Free(argc, argv);
      return -1;
    } 
    
    id_start = argv[2];
    
    x = atof(argv[3]);
    y = atof(argv[4]);
    start_angle = atof(argv[5]);
    sweep = atof(argv[6]);
    
    if (sweep <= 0.0) {
      ARGCV_Free(argc, argv);
      return -1;
    } 
    
    reference = argv[7];
    
  } else if (strcasecmp(argv[0], "CreateBumper") == 0) { 
    if (argc < 7) {
      ARGCV_Free(argc, argv);
      return -1;
    } 
    
    id_start = NULL;
    n = 1;
    id = argv[1];
    x = atof(argv[2]);
    y = atof(argv[3]);
    start_angle = atan2(atof(argv[4]), atof(argv[5]));
    reference = argv[6];
    sweep = 0.0;
  } 

  if (*bindex + n > N_MAX_BUMPER_COUNT) {
    fprintf(stderr, "Bumper (warning): only %d bumpers per set allowed.\n",
	    N_MAX_BUMPER_COUNT);
    n = N_MAX_BUMPER_COUNT - 1 - *bindex;
  } 
  
  for (i = 0; i < n; i++) {
    if (id_start != NULL) {
      tmp = (char *) calloc(strlen(set_id) + strlen(id_start) + 
			    ((i != 0) ? 1 + ((int) log10(i)) : 1) + 2, 1);
      carmen_test_alloc(tmp);
      sprintf(tmp, "%s:%s%u", set_id, id_start, i);
    } else { 
      tmp = (char *) calloc(strlen(set_id) + strlen(id) + 2, 1);
      carmen_test_alloc(tmp);
      sprintf(tmp, "%s:%s", set_id, id);	
    } 
    
    xset->Bumper[*bindex].ID = tmp;

    xset->Bumper[*bindex].Configuration = 
      (double *)calloc(9, sizeof(double));
    carmen_test_alloc(xset->Bumper[*bindex].Configuration);

    mysincos(i / (double) n * sweep + start_angle, &sine, &cosine);
    list_fill(xset->Bumper[*bindex].Configuration, 3, 3, 
	      cosine, -sine, x, sine, cosine, y, 0.0, 0.0, 1.0);

    xset->Bumper[*bindex].Reference = strdup(reference);
    (*bindex)++;
  } 

  ARGCV_Free(argc, argv);

  return 0;
}

void BUMP_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
  char *const_string; 
  char **set_argv; 
  char **bump_argv; 
  unsigned short s; 
  unsigned short bumper_index; 
  int set_argc; 
  int bump_argc; 
  int argc_index; 
  struct N_XBumperSet *xset; 

  const_string = SETUP_GetValue("[bumper]bumper_sets");
  assert(const_string != ((void *)0));

  ARGCV_Create(const_string, &set_argc, &set_argv);
  
  if (set_argc > N_MAX_BUMPER_SET_COUNT) {
    fprintf(stderr, "Setup: too many bumper sets in %s.\n"
	    "  Using the first %d.\n", "[bumper]bumper_sets", 
	    N_MAX_BUMPER_SET_COUNT);
    rs->BumperController.BumperSetCount = N_MAX_BUMPER_SET_COUNT;
  } else {
    rs->BumperController.BumperSetCount = set_argc;
  } 
  
  
  for (s = 0; s < rs->BumperController.BumperSetCount; s++) {
    xset = &(rs_ext->BumperController.BumperSet[s]);
    if ((const_string = SETUP_ExtGetValue(set_argv[s], "bumpers")) == NULL) {
      rs->BumperController.BumperSet[s].BumperCount = 0;
    } else {
      ARGCV_Create(const_string, &bump_argc, &bump_argv);
      argc_index = 0;
      bumper_index = 0; 
      while (bumper_index < N_MAX_BUMPER_COUNT) { 
	if (argc_index == bump_argc)
	  break;
	
	const_string = SETUP_ExtGetValue(set_argv[s], bump_argv[argc_index]);
	if (const_string == NULL) {
	  fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
		  set_argv[s], bump_argv[argc_index]);
	  argc_index++;	  
	  continue;
	} 

	if (ParseBumperCommand(xset, &bumper_index, set_argv[s], 
			       const_string) < 0) {
	  fprintf(stderr, "Bumper (warning): invalid bumper "
		  "specification after [%s]%s.\n",
		  set_argv[s], bump_argv[argc_index]);
	} 
	argc_index++;
      }
      
      rs->BumperController.BumperSet[s].BumperCount = bumper_index;
      
      ARGCV_Free(bump_argc, bump_argv);
    }
  } 

  ARGCV_Free(set_argc, set_argv);
  
  return ;
}

static
void ParseInfraredCommand(struct N_XInfrared *xir, char *command, 
			  char *base_id)
{
  int argc; 
  char **argv; 
  double x; 
  double y; 
  double sine; 
  double cosine; 

  ARGCV_Create(command, &argc, &argv);

  assert(argc >= 7);

  xir->ID = calloc(strlen(base_id) + strlen(argv[1]) + 2, 1);
  carmen_test_alloc(xir->ID);
  sprintf(xir->ID, "%s:%s", base_id, argv[1]);
  
  xir->Reference = strdup(argv[6]);

  x = atof(argv[2]);
  y = atof(argv[3]);
  cosine = atof(argv[4]);
  sine = atof(argv[5]);
  
  xir->Configuration = (double *)calloc(9, sizeof(double));
  carmen_test_alloc(xir->Configuration);
  list_fill(xir->Configuration, 3, 3, 
	    cosine, -sine, x, sine, cosine, y, 0.0, 0.0, 1.0);
  
  ARGCV_Free(argc, argv);
  
  return;
}

void INF_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
  unsigned short s; 
  unsigned short ir; 
  char *const_string; 
  char **set_argv; 
  char **ir_argv; 
  int set_argc; 
  int ir_argc; 
  struct N_XInfraredSet *xset; 

  const_string = SETUP_GetValue("[infrared]infrared_sets");
  
  assert(const_string != ((void *)0));

  ARGCV_Create(const_string, &set_argc, &set_argv);

  if (set_argc > N_MAX_INFRARED_SET_COUNT) {
    printf("Warning: too many infrared sets in %s.\n"
	   "  Using the first %d.\n", "[infrared]infrared_sets", 
	    N_MAX_INFRARED_SET_COUNT);
    rs->InfraredController.InfraredSetCount = N_MAX_INFRARED_SET_COUNT;
  } else {
    rs->InfraredController.InfraredSetCount = set_argc;
  } 







  for (s = 0; s < rs->InfraredController.InfraredSetCount; s++) {
    xset = &(rs_ext->InfraredController.InfraredSet[s]);

    const_string = SETUP_ExtGetValue(set_argv[s], "main_lobe");
    assert(const_string != ((void *)0));
    xset->MainLobe = atof(const_string);

    const_string = SETUP_ExtGetValue(set_argv[s], "range");
    assert(const_string != ((void *)0));
    xset->Range = atof(const_string);



    const_string = SETUP_ExtGetValue(set_argv[s], "infrareds");
    assert(const_string != ((void *)0));
    ARGCV_Create(const_string, &ir_argc, &ir_argv);

    if (ir_argc > N_MAX_INFRARED_COUNT) {
      printf("Warning: too many infrareds in [%s]%s.\n"
	     "  Using the first %d.\n", set_argv[s], "infrareds",
	     N_MAX_INFRARED_COUNT);
      rs->InfraredController.InfraredSet[s].InfraredCount = 
	N_MAX_INFRARED_COUNT;
    } else {
      rs->InfraredController.InfraredSet[s].InfraredCount = ir_argc;
    } 

    for (ir = 0; ir < rs->InfraredController.InfraredSet[s].InfraredCount;
	 ir++) {
      if ((const_string = SETUP_ExtGetValue(set_argv[s], ir_argv[ir])) == 
	  NULL) {
	fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
		set_argv[s], ir_argv[ir]);
 
      } else { 
	ParseInfraredCommand(&(xset->Infrared[ir]), const_string,
			     set_argv[s]);
      } 
    } 
    ARGCV_Free(ir_argc, ir_argv);
  }
  ARGCV_Free(set_argc, set_argv);
  
  return ;
}