base_main.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 Foobar; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

#include <carmen/carmen.h>
#include "base_low_level.h"

#ifdef BASE_HAS_ARM
#include <carmen/arm_messages.h> 
#include "arm_low_level.h"
#endif

void x_ipcRegisterExitProc(void (*)(void));

static int moving = 0;
static double current_acceleration = 0;
static double deceleration;
static carmen_robot_config_t robot_config;
static carmen_base_odometry_message odometry;
static carmen_base_binary_data_message binary_data;
static double reset_time = 0;
static double relative_wheelbase;
static double relative_wheelsize;

static int use_hardware_integrator = 1;
static int use_sonar = 1;
static carmen_base_sonar_message sonar;
static int sonar_state = 0;
static double *ranges = NULL;
static carmen_point_t *positions = NULL;
static int num_sonar_ranges;

static carmen_base_bumper_message bumper;

static double last_motion_command = 0;

static double motion_timeout = 1;
static int odometry_inverted;

static char *model_name;
static char *dev_name;

#ifdef BASE_HAS_ARM
static int num_arm_joints;
static carmen_arm_state_message arm_state;
#endif

static int 
initialize_robot(void)
{
  int result;

  result = carmen_base_direct_initialize_robot
    (model_name, dev_name);
  if (result < 0) 
    return -1;
  

  if (use_sonar) {
    num_sonar_ranges = carmen_base_direct_sonar_on();
    if(num_sonar_ranges < 0) 
      return -1;
    if (ranges == NULL)
      free(ranges);
    ranges = (double *)calloc(num_sonar_ranges, sizeof(double));
    carmen_test_alloc(ranges);
    positions = (carmen_point_t *)
      calloc(num_sonar_ranges, sizeof(carmen_point_t));
    carmen_test_alloc(positions);
    sonar_state = 1;
  } else {
    result = carmen_base_direct_sonar_off();
    if(result < 0) 
      return -1;
  }

  result = carmen_base_direct_reset();
  if (result < 0) 
    return -1;

  reset_time = carmen_get_time();
  odometry.host = carmen_get_host();

  binary_data.host = carmen_get_host();
  binary_data.size = 0;
  
  return 0;
}

#ifdef BASE_HAS_ARM

static void arm_command_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
				void *clientData __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;
  carmen_arm_command_message msg;
  FORMATTER_PTR formatter;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &msg,
			   sizeof(carmen_arm_command_message));
  IPC_freeByteArray(callData);
  
  if (msg.num_joints > 0) {
    carmen_arm_direct_set(msg.joint_angles, msg.num_joints);
    free(msg.joint_angles);
  }
}

static void arm_query_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
			      void *clientData __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;

  IPC_freeByteArray(callData);

  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_respondData(msgRef, CARMEN_ARM_STATE_NAME, &arm_state);
}

static void initialize_arm_message(void)
{
  int err;

  carmen_param_allow_unfound_variables(FALSE); 
  err = carmen_param_get_int("arm_num_joints", &num_arm_joints, NULL);
  if (err < 0)
    carmen_die("\nThe base_has_arm parameter is on, but the arm_num_joints "
	       "parameter is\nmissing. Please set the arm_num_joints "
	       "parameter.\n");
  arm_state.num_joints = num_arm_joints;
  if (num_arm_joints > 0) {
    arm_state.joint_angles = (double *)calloc(num_arm_joints, sizeof(double));
    carmen_test_alloc(arm_state.joint_angles);

    if(carmen_arm_direct_num_currents(num_arm_joints)) {
      arm_state.flags |= CARMEN_ARM_HAS_CURRENT_STATES;
      arm_state.num_currents = carmen_arm_direct_num_currents(num_arm_joints);
      arm_state.joint_currents = (double*)calloc
	( arm_state.num_currents, sizeof( double ) );
      carmen_test_alloc( arm_state.joint_currents );
    }
    if(carmen_arm_direct_num_velocities(num_arm_joints)) {
      arm_state.flags |= CARMEN_ARM_HAS_ANGULAR_VEL_STATES;
      arm_state.num_vels = carmen_arm_direct_num_velocities(num_arm_joints);
      arm_state.joint_angular_vels = (double*)calloc
	( arm_state.num_vels, sizeof( double ) );
      carmen_test_alloc( arm_state.joint_angular_vels );
    }
  }

  err = IPC_defineMsg(CARMEN_ARM_COMMAND_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_ARM_COMMAND_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_ARM_COMMAND_NAME);

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

  err = IPC_defineMsg(CARMEN_ARM_STATE_NAME, IPC_VARIABLE_LENGTH,
                      CARMEN_ARM_STATE_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
		       CARMEN_ARM_STATE_NAME);

  err = IPC_subscribe(CARMEN_ARM_COMMAND_NAME, arm_command_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", 
		       CARMEN_ARM_COMMAND_NAME);
  IPC_setMsgQueueLength(CARMEN_ARM_COMMAND_NAME, 1);

  err = IPC_subscribe(CARMEN_ARM_QUERY_NAME, arm_query_handler, NULL);
  carmen_test_ipc_exit(err, "Could not subscribe", 
                       CARMEN_ARM_QUERY_NAME);
  IPC_setMsgQueueLength(CARMEN_ARM_QUERY_NAME, 100);
}

#endif

static void
initialize_sonar_message(carmen_base_sonar_message *sonar)
{
  double sensor_angle;

  carmen_param_set_module("robot");
  carmen_param_get_double("sensor_angle", &sensor_angle, NULL);
  
  sonar->sensor_angle = sensor_angle;
  sonar->timestamp = 0.0;
  strncpy(sonar->host, odometry.host, 10);
}

static void
handle_sonar_change(char *module __attribute__ ((unused)), 
		    char *variable __attribute__ ((unused)), 
		    char *value __attribute__ ((unused)))
{
  int err;

  if (use_sonar && !sonar_state) {
    initialize_sonar_message(&sonar);
    do {
      num_sonar_ranges = carmen_base_direct_sonar_on();
      if (num_sonar_ranges < 0)
	initialize_robot();
    } 
    while (num_sonar_ranges < 0);

    ranges = (double *)calloc(num_sonar_ranges, sizeof(double));
    carmen_test_alloc(ranges);
    positions = (carmen_point_t *)
      calloc(num_sonar_ranges, sizeof(carmen_point_t));
    carmen_test_alloc(positions);
    sonar_state = 1;
  } else if (!use_sonar && sonar_state) {
    if (ranges) 
      free(ranges);
    if (positions)
      free(positions);
    do {
      err = carmen_base_direct_sonar_off();
      if (err < 0)
	initialize_robot();
    } while (err < 0);
    sonar_state = 0;
  } /* end of if (use_sonar && !sonar_state) ... else if (sonar_state) */
}

static int
read_parameters(int argc, char **argv)
{
  int num_items;

  carmen_param_t param_list[] = {
    {"base", "dev", CARMEN_PARAM_STRING, &dev_name, 0, NULL},
    {"base", "model", CARMEN_PARAM_STRING, &model_name, 0, NULL},
    {"base", "motion_timeout", CARMEN_PARAM_DOUBLE, &motion_timeout, 0, NULL},
    {"base", "use_hardware_integrator", CARMEN_PARAM_ONOFF, 
     &use_hardware_integrator, 0, NULL},
    {"robot", "odometry_inverted", CARMEN_PARAM_ONOFF, &odometry_inverted, 
     0, NULL},
    {"robot", "use_sonar", CARMEN_PARAM_ONOFF, &use_sonar, 1, 
     handle_sonar_change}, 
    {"robot", "acceleration", CARMEN_PARAM_DOUBLE, 
     &(robot_config.acceleration), 1, NULL},
    {"robot", "deceleration", CARMEN_PARAM_DOUBLE, 
     &(deceleration), 1, NULL}};

  carmen_param_t extra_params[] = {
    {"base", "relative_wheelsize", CARMEN_PARAM_DOUBLE, 
     &relative_wheelsize, 0, NULL},
    {"base", "relative_wheelbase", CARMEN_PARAM_DOUBLE, 
     &relative_wheelbase, 0, NULL},
  };

  num_items = sizeof(param_list)/sizeof(param_list[0]);
  carmen_param_install_params(argc, argv, param_list, num_items);

  if (use_sonar)
    initialize_sonar_message(&sonar);
  else
    memset(&sonar, 0, sizeof(carmen_base_sonar_message));

  if (!use_hardware_integrator) {
    num_items = sizeof(extra_params)/sizeof(extra_params[0]);
    carmen_param_install_params(argc, argv, extra_params, num_items);      
  }

#ifdef BASE_HAS_ARM  
  initialize_arm_message();
#endif

  if (robot_config.acceleration > deceleration) 
    carmen_die("ERROR: robot_deceleration must be greater or equal than "
	       "robot_acceleration\n");
  
  return 0;
}

static void 
velocity_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
		 void *clientData __attribute__ ((unused)))
{
  IPC_RETURN_TYPE err;
  carmen_base_velocity_message vel;
  int base_err;
  FORMATTER_PTR formatter;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &vel,
                     sizeof(carmen_base_velocity_message));
  IPC_freeByteArray(callData);

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

  if(vel.tv == 0 && vel.rv == 0) 
    {
      if (moving) 
	{
	  do 
	    {
	      base_err = carmen_base_direct_set_deceleration(deceleration);
	      if (base_err < 0)
		initialize_robot();
	    } 
	  while (base_err < 0);
	  moving = 0;
	}
      carmen_warn("S");
    }
  else if (!moving) 
    {
      moving = 1;
      current_acceleration = robot_config.acceleration;
      do 
	{
	  base_err = carmen_base_direct_set_acceleration(current_acceleration);
	  if (base_err < 0)
	    initialize_robot();
	} 
      while (base_err < 0);
      carmen_warn("V");
    }  

  if (odometry_inverted)
    vel.tv *= -1;
  if (!use_hardware_integrator)
    {
      vel.tv /= relative_wheelsize;
      
      vel.rv /= relative_wheelsize;
      vel.rv /= relative_wheelbase;
    }

  do