/********************************************************* * * 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"#endifvoid 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_ARMstatic int num_arm_joints;static carmen_arm_state_message arm_state;#endifstatic 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_ARMstatic 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);}#endifstatic voidinitialize_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->cone_angle = sensor_angle;  sonar->timestamp = 0.0;  strncpy(sonar->host, odometry.host, 10);}static voidhandle_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 intread_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