base_main.c

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

    {
      base_err = carmen_base_direct_set_velocity(vel.tv, vel.rv);
      last_motion_command = carmen_get_time();
      if (base_err < 0)
	initialize_robot();
    } 
  while (base_err < 0);
}

static void
binary_command_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
                       void *clientData __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;
  carmen_base_binary_data_message msg;
  FORMATTER_PTR formatter;
  int base_err;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &msg,
                           sizeof(carmen_base_binary_data_message));
  IPC_freeByteArray(callData);

  carmen_test_ipc_return(err, "Could not unmarshall", 
                         IPC_msgInstanceName(msgRef));

  if (msg.size > 0)
    {
      do 
	{
	  base_err = carmen_base_direct_send_binary_data(msg.data, msg.size);
	  if (base_err < 0)
	    initialize_robot();
	} 
      while (base_err < 0);
      free(msg.data);
    }
}

static void 
reset_odometry()
{
 printf("Odometry Reset...\n");
  odometry.x=0;
  odometry.y=0;
  odometry.theta=0;
}

static void reset_handler(MSG_INSTANCE msgRef __attribute__ ((unused)), 
			  BYTE_ARRAY callData,
			  void *clientData __attribute__ ((unused)))
{
  int base_err;

  IPC_freeByteArray(callData);

  do {
    base_err = carmen_base_direct_reset();
    //Edsinger: Reset hack
    reset_odometry();
    if (base_err < 0)
      initialize_robot();
  } while (base_err < 0);
}

int 
carmen_base_initialize_ipc(void)
{
  IPC_RETURN_TYPE err;

  /* define messages created by base */
  err = IPC_defineMsg(CARMEN_BASE_ODOMETRY_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_ODOMETRY_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_ODOMETRY_NAME);

  err = IPC_defineMsg(CARMEN_BASE_SONAR_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_SONAR_FMT);
  carmen_test_ipc_exit(err, "Could not define IPC message", 
		       CARMEN_BASE_SONAR_NAME);

  err = IPC_defineMsg(CARMEN_BASE_BUMPER_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_BUMPER_FMT);
  carmen_test_ipc_exit(err, "Could not define IPC message", 
		       CARMEN_BASE_BUMPER_NAME);

  err = IPC_defineMsg(CARMEN_BASE_VELOCITY_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_VELOCITY_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_VELOCITY_NAME);

  err = IPC_defineMsg(CARMEN_BASE_RESET_OCCURRED_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_DEFAULT_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_RESET_OCCURRED_NAME);

  err = IPC_defineMsg(CARMEN_BASE_RESET_COMMAND_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_DEFAULT_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
		       CARMEN_BASE_RESET_COMMAND_NAME);

  err = IPC_defineMsg(CARMEN_BASE_BINARY_COMMAND_NAME, 
                      IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_BINARY_COMMAND_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
                       CARMEN_BASE_BINARY_COMMAND_NAME);

  err = IPC_defineMsg(CARMEN_BASE_BINARY_DATA_NAME, 
                      IPC_VARIABLE_LENGTH,
                      CARMEN_BASE_BINARY_DATA_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
                       CARMEN_BASE_BINARY_DATA_NAME);

  /* setup incoming message handlers */

  err = IPC_subscribe(CARMEN_BASE_VELOCITY_NAME, velocity_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", CARMEN_BASE_VELOCITY_NAME);
  IPC_setMsgQueueLength(CARMEN_BASE_VELOCITY_NAME, 1);

  err = IPC_subscribe(CARMEN_BASE_RESET_COMMAND_NAME, reset_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", 
		       CARMEN_BASE_RESET_COMMAND_NAME);
  IPC_setMsgQueueLength(CARMEN_BASE_RESET_COMMAND_NAME, 1);

  err = IPC_subscribe(CARMEN_BASE_BINARY_COMMAND_NAME, 
                      binary_command_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", 
                       CARMEN_BASE_BINARY_COMMAND_NAME);
  IPC_setMsgQueueLength(CARMEN_BASE_BINARY_COMMAND_NAME, 1);

  return IPC_No_Error;
}

int 
carmen_base_start(int argc, char **argv)
{
  if (read_parameters(argc, argv) < 0)
    return -1;

  if (carmen_base_initialize_ipc() < 0) {
    carmen_warn("\nError: Could not initialize IPC.\n");
    return -1;
  }
  
  if(initialize_robot() < 0) {
    carmen_warn("\nError: Could not connect to robot on %s. "
		"Did you remember to turn the base on?\n", dev_name);
    carmen_warn("     : Are the permissions on %s correct (0666)?\n", dev_name);
    carmen_warn("     : Do you have the line speed set correctly?\n");
    carmen_warn("     : Is the robot cable plugged in securely?\n");
    return -1;
  }

  odometry.host = carmen_get_host();

  return 0;
}

static void
integrate_odometry(double displacement, double rotation, double tv, double rv)
{
  displacement *= relative_wheelsize;
  rotation *= relative_wheelsize;
  rotation *= relative_wheelbase;
      
  odometry.tv = tv * relative_wheelsize;
  odometry.rv = rv * relative_wheelsize / relative_wheelbase;
  
  if (odometry_inverted) {
    odometry.tv *= -1;
    displacement *= -1;
  }

  /*Giorgio 03.03.2004
  added Rung Kutta integration.
  */
  /*
  odometry.x += displacement * cos (odometry.theta);
  odometry.y += displacement * sin (odometry.theta);
  */
  
  odometry.x += displacement * cos (odometry.theta+0.5*rotation);
  odometry.y += displacement * sin (odometry.theta+0.5*rotation);
  odometry.theta = carmen_normalize_theta(odometry.theta+rotation);
}


int 
carmen_base_run(void) 
{
  IPC_RETURN_TYPE err;
  int index;
  static carmen_base_reset_occurred_message reset = {0, 0};  
  int base_err;
  double tv, rv;
  double displacement, rotation;
  carmen_base_binary_data_message binary_data;

  if (reset_time > reset.timestamp) {
    reset.timestamp = reset_time;
    reset.host = carmen_get_host();
    err = IPC_publishData(CARMEN_BASE_RESET_OCCURRED_NAME, &reset);
    carmen_test_ipc_exit(err, "Could not publish", CARMEN_BASE_RESET_OCCURRED_NAME);  
    return 1;
  }  

  if (moving && carmen_get_time() - last_motion_command > motion_timeout) {
    moving = 0;
    do {
      base_err = carmen_base_direct_set_deceleration(deceleration);
      if (base_err < 0)
	initialize_robot();
    } while (base_err < 0);
    carmen_warn("T");      
    do {
      base_err = carmen_base_direct_set_velocity(0, 0);
      if (base_err < 0)
	initialize_robot();
    } while (base_err < 0);      
  }

  do {
    double packet_timestamp=0.;
    base_err = carmen_base_direct_update_status(&packet_timestamp);
    if (packet_timestamp==0.)
    	odometry.timestamp = carmen_get_time();
    else
    	odometry.timestamp = packet_timestamp;
    if (base_err < 0)
      initialize_robot();
  } while (base_err < 0); 

  carmen_base_direct_get_binary_data(&(binary_data.data), &(binary_data.size));
  binary_data.host = carmen_get_host();
  /*
  Giorgio 03.03.2006
	The binary data timestamp should be considered when reading the 1st byte of the packet.
  */
  binary_data.timestamp = carmen_get_time();
  IPC_publishData(CARMEN_BASE_BINARY_DATA_NAME, &binary_data);

  if (base_err > 0)
    return 2;

  do {
    if (!use_hardware_integrator) {
      base_err = carmen_base_direct_get_state
	(&displacement, &rotation, &tv, &rv);
      if (base_err < 0)
	initialize_robot();
      else
	integrate_odometry(displacement, rotation, tv, rv);
    } else {
      base_err = carmen_base_direct_get_integrated_state
	(&(odometry.x), &(odometry.y), &(odometry.theta), &(odometry.tv), 
	 &(odometry.rv));
      if (base_err < 0)
	initialize_robot();
      else {
	if (odometry_inverted) {
	  odometry.tv *= -1;
	  odometry.x *= -1;
	  odometry.y *= -1;
	}
      }
    }
  } while (base_err < 0);

  if (use_sonar && sonar_state) {
    carmen_base_direct_get_sonars(ranges, positions, num_sonar_ranges);
    sonar.num_sonars = num_sonar_ranges;
    for (index = 0; index < sonar.num_sonars; index++) {
      sonar.range = ranges;
      sonar.positions = positions;
    }
  }
  
  err = IPC_publishData(CARMEN_BASE_ODOMETRY_NAME, &odometry);
  carmen_test_ipc_exit(err, "Could not publish", 
		       CARMEN_BASE_ODOMETRY_NAME);
  
  if (use_sonar) {
    carmen_warn("s");  
    sonar.timestamp = carmen_get_time();
    sonar.host = carmen_get_host();

    err = IPC_publishData(CARMEN_BASE_SONAR_NAME, &sonar);
    carmen_test_ipc_exit(err, "Could not publish", 
			 CARMEN_BASE_SONAR_NAME);
  }

  bumper.num_bumpers = 
    carmen_base_direct_get_bumpers(bumper.state, bumper.num_bumpers);
  if (bumper.state == NULL) {
    bumper.state = (unsigned char *)calloc(bumper.num_bumpers, sizeof(char));
    carmen_test_alloc(bumper.state);
  }

  if (bumper.num_bumpers > 0) {
    bumper.timestamp = carmen_get_time();
    bumper.host = carmen_get_host();
    err = IPC_publishData(CARMEN_BASE_BUMPER_NAME, &bumper);
    carmen_test_ipc_exit(err, "Could not publish", 
			 CARMEN_BASE_BUMPER_NAME);
  }

#if BASE_HAS_ARM
  if (num_arm_joints > 0) 
    carmen_arm_direct_get_state(arm_state.joint_angles, 
				arm_state.joint_currents,
				arm_state.joint_angular_vels, 
				&arm_state.gripper_closed, num_arm_joints);

  arm_state.timestamp = carmen_get_time();
  arm_state.host = carmen_get_host();
  err = IPC_publishData(CARMEN_ARM_STATE_NAME, &arm_state);
#endif

  return 1;
}

void 
carmen_base_shutdown(void)
{
  carmen_verbose("\nShutting down robot...");
  sleep(1);
  if (use_sonar && sonar_state)
    carmen_base_direct_sonar_off();
  carmen_verbose("done.\n");
  carmen_base_direct_shutdown_robot();
  exit(0);
}

void 
carmen_base_emergency_crash(int x __attribute__ ((unused)))
{
  carmen_base_direct_set_velocity(0.0, 0.0);
}

static void
shutdown_base(int signo __attribute__ ((unused)))
{
  carmen_base_shutdown();
}

int 
main(int argc, char **argv)
{
  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);

  signal(SIGINT, shutdown_base);
  signal(SIGSEGV, carmen_base_emergency_crash);

  x_ipcRegisterExitProc(carmen_base_shutdown);

  if (carmen_base_start(argc, argv) < 0)
    exit(-1);

  while(1) {
    if (carmen_base_run() == 1)
      fprintf(stderr, ".");
    carmen_ipc_sleep(0.1);
  }

  return 0;
}