rflex.cc

机器人仿真软件

/*  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program 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.
 *
 *  This program 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 this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* notes:
 * the Main thread continues running when no-one is connected
 * this we retain our odometry data, whether anyone is connected or not
 */

/* Modified by Toby Collett, University of Auckland 2003-02-25
 * Added support for bump sensors for RWI b21r robot, uses DIO
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_rflex rflex
 * @brief RWI mobile robots

The rflex driver is used to control RWI robots by directly communicating
with RFLEX onboard the robot (i.e., Mobility is bypassed).  To date,
these drivers have been tested on an ATRV-Jr, but they should
work with other RFLEX-controlled robots: you will have to determine some
parameters to set in the config file, however.

As of March 2003 these drivers have been modified to support the
b21r robot, Currently additional support has been added for the @ref
interface_power interface and @ref interface_bumper
interface. For the pan tilt unit on the b21r please refer to
the @ref driver_ptu46 driver.

@par Compile-time dependencies

- none

@par Provides

The rflex driver provides the following device interfaces, some of them named:

- @ref interface_position2d : This interface returns odometry data,
  and accepts velocity commands.
- "sonar" @ref interface_sonar : Range data from the sonar array
- "sonar2" @ref interface_sonar : Range data from the second sonar array
- @ref interface_ir
- @ref interface_bumper
- @ref interface_power
- @ref interface_aio
- @ref interface_dio

@par Supported configuration requests

- The @ref interface_position2d interface supports:
  - PLAYER_POSITION_SET_ODOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_VELOCITY_MODE_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_GET_GEOM_REQ
- The @ref interface_ir interface supports:
  - PLAYER_IR_POWER_REQ
  - PLAYER_IR_POSE_REQ
- The "sonar" @ref interface_sonar interface supports:
  - PLAYER_SONAR_POWER_REQ
  - PLAYER_SONAR_GET_GEOM_REQ
- The "sonar2" @ref interface_sonar interface supports:
  - PLAYER_SONAR_POWER_REQ
  - PLAYER_SONAR_GET_GEOM_REQ
- The @ref interface_bumper interface supports:
  - PLAYER_BUMPER_GET_GEOM_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyR0"
  - Serial port used to communicate with the robot.
- m_length (float)
  - Default: 0.5
  - Length of the robot in meters
- m_width (float)
  - Default: 0.5
  - Width of the robot in meters
- odo_distance_conversion (float)
  - Default: 0
  - Odometry conversion. See Note 1.
- odo_angle_conversion (float)
  - Default: 0
  - Odometry conversion. See Note 2.
- default_trans_acceleration (float)
  - Default: 0.1
  - Set translational acceleration, in m.
- default_rot_acceleration (float)
  - Default: 0.1
  - Set rotational acceleration, in radians.
- rflex_joystick (integer)
  - Default: 0
  - Enables joystick control via the rflex controller
- rflex_joy_pos_ratio (float)
  - Default: 0
  - Joystick to movement conversion ratio
- rflex_joy_ang_ratio (float)
  - Default: 0
  - Joystick to movement conversion ratio
- range_distance_conversion (float)
  - Default: 1476
  - Sonar range conversion factor. See Note 7.
- max_num_sonars (integer)
  - Default: 64
  - See Note 4
- num_sonars (integer)
  - Default: 24
  - See Note 4
- sonar_age (integer)
  - Default: 1
  - Prefiltering parameter. See Note 3.
- num_sonar_banks (integer)
  - Default: 8
  - See Note 4
- num_sonars_possible_per_bank (integer)
  - Default: 16
  - See Note 4
- num_sonars_in_bank (integer tuple)
  - Default: [ 8 8 8 ... ]
  - See Note 4
- sonar_echo_delay (integer)
  - Default: 3000
  - Sonar configuration parameters
- sonar_ping_delay (integer)
  - Default: 0
  - Sonar configuration parameters
- sonar_set_delay (integer)
  - Default: 0
  - Sonar configuration parameters
- mrad_sonar_poses (tuple float)
  - Default: [ 0 0 0 ... ]
  - Sonar positions and directions.  See Note 6.
- sonar_2nd_bank_start (integer)
  - Default: 0
  - Address of the second sonar bank (lower bank on the b21r)
- pose_count (integer)
  - Default: 8
  - Total Number of IR sensors
- rflex_base_bank (integer)
  - Default: 0
  - Base IR Bank
- rflex_bank_count (integer)
  - Default: 0
  - Number of banks in use
- ir_min_range (float)
  - Default: 0.100
  - Min range of ir sensors (m) (Any range below this is returned as 0)
- ir_max_range (float)
  - Default: 0.800
  - Max range of ir sensors (m) (Any range above this is returned as max)
- rflex_banks (float tuple)
  - Default: [ 0 0 0 ... ]
  - Number of IR sensors in each bank
- poses (float tuple)
  - Default: [ 0 0 0 ... ]
  - x,y,theta of IR sensors (m, m, deg)
- rflex_ir_calib (float tuple)
  - Default: [ 1 1 ... ]
  - IR Calibration data (see Note 8)
- bumper_count (integer)
  - Default: 0
  - Number of bumper panels
- bumper_def (float tuple)
  - Default: [ 0 0 0 0 0 ... ]
  - x,y,theta,length,radius (m,m,deg,m,m) for each bumper
- rflex_bumper_address (integer)
  - Default: 0x40
  - The base address of first bumper in the DIO address range
- rflex_bumper_style (string)
  - Default: "addr"
  - Bumpers can be defined either by addresses or a bit mask
- rflex_power_offset (float)
  - Default: 0
  - The calibration constant for the power calculation in volts

@par Notes
-# Since the units used by the Rflex for odometry appear to be completely
   arbitrary, this coefficient is needed to convert to meters: m =
   (rflex units) / (odo_distance_conversion).  These arbitrary units
   also seem to be different on each robot model. I'm afraid you'll
   have to determine your robot's conversion factor by driving a known
   distance and observing the output of the RFlex.
-# Conversion coefficient
   for rotation odometry: see odo_distance_conversion. Note that
   heading is re-calculated by the Player driver since the RFlex is not
   very accurate in this respect. See also Note 1.
-# Used for prefiltering:
   the standard Polaroid sensors never return values that are closer
   than the closest obstacle, thus we can buffer locally looking for the
   closest reading in the last "sonar_age" readings. Since the servo
   tick here is quite small, you can still get pretty recent data in
   the client.
-# These values are all used for remapping the sonars from Rflex indexing
   to player indexing. Individual sonars are enumerated 0-15 on each
   board, but at least on my robots each only has between 5 and 8 sonar
   actually attached.  Thus we need to remap all of these indexes to
   get a contiguous array of N sonars for Player.
     - max_num_sonars is the maximum enumeration value+1 of
       all sonar meaning if we have 4 sonar boards this number is 64.
     - num_sonars is the number of physical sonar sensors -
       meaning the number of ranges that will be returned by Player.  -
       num_sonar_banks is the number of sonar boards you have.
     - num_sonars_possible_per_bank is probobly 16 for all
       robots, but I included it here just in case. this is the number of
       sonar that can be attached to each sonar board, meaning the maximum
       enumeration value mapped to each board.  - num_sonars_in_bank
       is the nubmer of physical sonar attached to each board in order -
       you'll notice on each the sonar board a set of dip switches, these
       switches configure the enumeration of the boards (ours are 0-3)
-# The first RFlex device (position, sonar or power) in the config file
   must include this option, and only the first device's value will be used.
-# This is about the ugliest way possible of telling Player where each
   sonar is mounted.  Include in the string groups of three values: "x1
   y1 th1 x2 y2 th2 x3 y3 th3 ...".  x and y are m and theta is radians,
in Player's robot coordinate system.
-# Used to convert between arbitrary sonar units to millimeters: m =
   sonar units / range_distance_conversion.
-# Calibration is in the form Range = a*Voltage^b and stored in the
   tuple as [a1 b1 a2 b2 ...] etc for each ir sensor.

@par Example

@verbatim
driver
(
  name "rflex"
  provides ["position2d:1" "bumper:0" "sonar::sonar:0" "sonar2::sonar:1" "power:0" "ir:0"]

  rflex_serial_port     "/dev/ttyR0"
  m_length      0.5
  m_width       0.5
  odo_distance_conversion   103000
  odo_angle_conversion    35000
  default_trans_acceleration  0.5
  default_rot_acceleration    0.017
  rflex_joystick      1
  rflex_joy_pos_ratio   6
  rflex_joy_ang_ratio   -0.01


  bumper_count    14
  bumper_def    [   -216.506351 125.000000 -210.000000 261.799388 250.000000 -0.000000 250.000000 -270.000000 261.799388 250.000000 216.506351 125.000000 -330.000000 261.799388 250.000000 216.506351 -125.000000 -390.000000 261.799388 250.000000 0.000000 -250.000000 -450.000000 261.799388 250.000000 -216.506351 -125.000000 -510.000000 261.799388 250.000000 -240.208678 -99.497692 -157.500000 204.203522 260.000000 -240.208678 99.497692 -202.500000 204.203522 260.000000 -99.497692 240.208678 -247.500000 204.203522 260.000000 99.497692 240.208678 -292.500000 204.203522 260.000000 240.208678 99.497692 -337.500000 204.203522 260.000000 240.208678 -99.497692 -382.500000 204.203522 260.000000 99.497692 -240.208678 -427.500000 204.203522 260.000000 -99.497692 -240.208678 -472.500000 204.203522 260.000000 ]
  rflex_bumper_address  64 # 0x40

  range_distance_conversion   1.476
  sonar_age       1
  sonar_echo_delay    30000
  sonar_ping_delay    0
  sonar_set_delay     0
  max_num_sonars      224
  num_sonars        48
  num_sonar_banks     14
  num_sonars_possible_per_bank  16
  num_sonars_in_bank    [4 4 4 4 4 4 3 3 3 3 3 3 3 3]
  # theta (rads), x, y (m) in robot coordinates (x is forward)
  mrad_sonar_poses  [     3.01069  -0.24786122    0.03263155     2.74889  -0.23096988    0.09567086     2.48709  -0.19833834   0.15219036     2.22529  -0.15219036   0.19833834     1.96350   -0.09567086   0.23096988     1.70170   -0.03263155   0.24786122     1.43990    0.03263155   0.24786122     1.17810    0.09567086   0.23096988     0.91630   0.15219036   0.19833834     0.65450   0.19833834   0.15219036     0.39270   0.23096988    0.09567086     0.13090   0.24786122    0.03263155    -0.13090   0.24786122   -0.03263155    -0.39270   0.23096988   -0.09567086    -0.65450   0.19833834  -0.15219036    -0.91630   0.15219036  -0.19833834    -1.17810    0.09567086  -0.23096988    -1.43990    0.03263155  -0.24786122    -1.70170   -0.03263155  -0.24786122    -1.96350   -0.09567086  -0.23096988    -2.22529  -0.15219036  -0.19833834    -2.48709  -0.19833834  -0.15219036    -2.74889  -0.23096988   -0.09567086    -3.01069  -0.24786122   -0.03263155       4.18879  -0.13000000  -0.22516660     3.92699  -0.18384776  -0.18384776     3.66519  -0.22516660  -0.13000000     3.40339  -0.25114071   -0.06729295     3.14159  -0.26000000     0.00000     2.87979  -0.25114071    0.06729295     2.61799  -0.22516660   0.13000000     2.35619  -0.18384776   0.18384776     2.09440  -0.13000000   0.22516660     1.83260   -0.06729295   0.25114071     1.57080     0.00000   0.26000000     1.30900    0.06729295   0.25114071     1.04720   0.13000000   0.22516660     0.78540   0.18384776   0.18384776     0.52360   0.22516660   0.13000000     0.26180   0.25114071    0.06729295     0.00000   0.26000000     0.00000    -0.26180   0.25114071   -0.06729295    -0.52360   0.22516660  -0.13000000    -0.78540   0.18384776  -0.18384776    -1.04720   0.13000000  -0.22516660    -1.30900    0.06729295  -0.25114071    -1.57080     0.00000  -0.26000000    -1.83260   -0.06729295  -0.25114071    -2.09440  -0.13000000  -0.22516660    -2.35619  -0.18384776  -0.18384776]
  sonar_2nd_bank_start  24

  rflex_power_offset    1.2 # volts

  rflex_base_bank 0
  rflex_bank_count 6
  rflex_banks [4 4 4 4 4 4]
  pose_count  24
  ir_min_range  0.100
  ir_max_range  0.800
  rflex_ir_calib  [ 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 4000 -2 ]
  poses [-0.247861 0.032632 -3.272492 -0.230970 0.095671 -3.534292 -0.198338 0.152190 -3.796091 -0.152190 0.198338 -4.057890 -0.095671 0.230970 -4.319690 -0.032632 0.247861 -4.581489 0.032632 0.247861 -4.843289 0.095671 0.230970 -5.105088 0.152190 0.198338 -5.366888 0.198338 0.152190 -5.628687 0.230970 0.095671 -5.890486 0.247861 0.032632 -6.152286 0.247861 -0.032632 -6.414085 0.230970 -0.095671 -6.675884 0.198338 -0.152190 -6.937684 0.152190 -0.198338 -7.199483 0.095671 -0.230970 -7.461283 0.032632 -0.247861 -7.723082 -0.032632 -0.247861 -7.984881 -0.095671 -0.230970 -8.246680 -0.152190 -0.198338 -8.508480 -0.198338 -0.152190 -8.770280 -0.230970 -0.095671 -9.032079 -0.247861 -0.032631 -9.293879 ]
)
@endverbatim

@author Matthew Brewer, Toby Collett
*/
/** @} */



#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <termios.h>
#include <assert.h>

#include "rflex.h"
#include "rflex_configs.h"

//rflex communications stuff
#include "rflex_commands.h"
#include "rflex-io.h"

#include <libplayercore/playercore.h>
extern PlayerTime* GlobalTime;

extern int               RFLEX::joy_control;

// help function to rotate sonar positions
void SonarRotate(double heading, double x1, double y1, double t1, double *x2, double *y2, double *t2)
{
  *t2 = t1 - heading;
  *x2 = x1*cos(heading) + y1*sin(heading);
  *y2 = -x1*sin(heading) + y1*cos(heading);
}


//NOTE - this is accessed as an extern variable by the other RFLEX objects
rflex_config_t rflex_configs;

/* initialize the driver.
 *
 * returns: pointer to new REBIR object
 */
Driver*
RFLEX_Init(ConfigFile *cf, int section)
{
  return (Driver *) new RFLEX( cf, section);
}