xr4000_control.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 <carmen/carmen.h>
#include "Nclient.h"
#include "xr4000_control.h"
#include "xr4000_ipc.h"
#include "xr4000.h"

typedef struct {
  double max_tv, max_rv;
  double t_accel, r_accel;
  struct N_RobotState *rs;
  struct N_Configuration *config;
} xr4000_t, *xr4000_p;

xr4000_t robot;
extern int robd_robot_id;

static void carmen_xr4000_publish_odometry(void)
{
  carmen_base_odometry_message odometry;
  double xv = robot.rs->AxisSet.Axis[N_XTRANSLATION].DesiredSpeed / 10.0;
  double yv = robot.rs->AxisSet.Axis[N_YTRANSLATION].DesiredSpeed / 10.0;
  double rv = robot.rs->AxisSet.Axis[N_ROTATION].DesiredSpeed / 1000.0;
  IPC_RETURN_TYPE err;

  /* get robot position */
  N_GetIntegratedConfiguration(1);
  odometry.x = robot.rs->Integrator.y / 1000.0;
  odometry.y = -robot.rs->Integrator.x / 1000.0;
  odometry.theta = 
    carmen_normalize_theta(robot.rs->Integrator.Rotation / 1000.0);
  odometry.tv = hypot(xv, yv);
  odometry.rv = rv;
  odometry.acceleration = robot.t_accel;
  odometry.timestamp = carmen_get_time();
  odometry.host = carmen_get_host();

  err = IPC_publishData(CARMEN_BASE_ODOMETRY_NAME, &odometry);
  carmen_test_ipc_exit(err, "Could not publish", CARMEN_BASE_ODOMETRY_NAME);
}

static void carmen_xr4000_execute_velocity(void)
{
  /* execute robot velocity */
  if(N_SetAxes(robd_robot_id) != N_NO_ERROR)
    carmen_warn("Could not set robot velocity.\n");
}

void carmen_xr4000_usage(char *progname, char *fmt, ...) 
{
  va_list args;

  if(fmt != NULL) {
    va_start(args, fmt);
    vfprintf(stderr, fmt, args);
    va_end(args);
    fprintf(stderr, "\n\n");
  }
  else
    fprintf(stderr, "\n");
  if(strrchr(progname, '/') != NULL) {
    progname = strrchr(progname, '/');
    progname++;
  }
  fprintf(stderr, "Usage: %s <args>\n", progname);
  fprintf(stderr, "\t-dev devname   - device connected to scout robot.\n");
  exit(-1);
}

void carmen_xr4000_set_velocity(double xv, double yv, double rv)
{
  /* set robot velocity */
  xv *= 1000;
  if(xv > XR4000_MAX_TV)
    xv = XR4000_MAX_TV;
  yv *= 1000;
  if(yv > XR4000_MAX_TV)
    yv = XR4000_MAX_TV;
  rv *= 1000;
  if(rv > XR4000_MAX_TV)
    rv = XR4000_MAX_TV;
  robot.rs->AxisSet.Axis[N_XTRANSLATION].DesiredSpeed = (long)(xv);
  robot.rs->AxisSet.Axis[N_XTRANSLATION].Update = TRUE;
  robot.rs->AxisSet.Axis[N_YTRANSLATION].DesiredSpeed = (long)(yv);
  robot.rs->AxisSet.Axis[N_YTRANSLATION].Update = TRUE;
  robot.rs->AxisSet.Axis[N_ROTATION].DesiredSpeed = (long)(rv);
  robot.rs->AxisSet.Axis[N_ROTATION].Update = TRUE;
}

void carmen_xr4000_start(void)
{
  /* get the robot state */
  robot.rs = N_GetRobotState(robd_robot_id);
  if(robot.rs == NULL)
    carmen_die("Error: could not get robot state.\n");

  /* set robot control parameters */
  robot.rs->AxisSet.Global = FALSE;
  robot.rs->AxisSet.Axis[N_XTRANSLATION].Mode = N_AXIS_VELOCITY;
  robot.rs->AxisSet.Axis[N_XTRANSLATION].Acceleration = XR4000_T_ACCEL;
  robot.rs->AxisSet.Axis[N_YTRANSLATION].Mode = N_AXIS_VELOCITY;
  robot.rs->AxisSet.Axis[N_YTRANSLATION].Acceleration = XR4000_T_ACCEL;
  robot.rs->AxisSet.Axis[N_ROTATION].Mode = N_AXIS_VELOCITY;
  robot.rs->AxisSet.Axis[N_ROTATION].Acceleration = XR4000_R_ACCEL;

  carmen_xr4000_set_velocity(0, 0, 0);
  carmen_xr4000_register_ipc_messages();
}

void carmen_xr4000_run(void)
{
  carmen_xr4000_execute_velocity();
  carmen_xr4000_publish_odometry();
}

static void carmen_xr4000_stop_robot(void)
{
  /* stop all robot motion */
  robot.rs->AxisSet.Axis[N_XTRANSLATION].Mode = N_AXIS_STOP;
  robot.rs->AxisSet.Axis[N_XTRANSLATION].Update = TRUE;
  robot.rs->AxisSet.Axis[N_YTRANSLATION].Mode = N_AXIS_STOP;
  robot.rs->AxisSet.Axis[N_YTRANSLATION].Update = TRUE;
  robot.rs->AxisSet.Axis[N_ROTATION].Mode = N_AXIS_STOP;
  robot.rs->AxisSet.Axis[N_ROTATION].Update = TRUE;
  if(N_SetAxes(robd_robot_id) != N_NO_ERROR)
    carmen_warn("Failure when stopping robot.\n");
}

void carmen_xr4000_shutdown(int sig)
{
  if(sig == SIGINT)
    carmen_xr4000_stop_robot();
}

void carmen_xr4000_emergency_crash(int sig __attribute ((unused)))
{
  carmen_xr4000_stop_robot();
}