linuxjoy.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2003  
 *     Brian Gerkey, 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
 *
 */

/*
 * Desc: Read data from a standard linux joystick
 * Author: Andrew Howard
 * Date: 25 July 2004
 * CVS: $Id: linuxjoy.cc,v 1.30 2006/02/27 18:19:03 gerkey Exp $
 *
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_linuxjoystick linuxjoystick
 * @brief Linux joystick

The linuxjoystick driver reads data from a standard Linux joystick and
provides the data via the @ref interface_joystick interface.
This driver can also control a @ref interface_position2d device by
converting joystick positions to velocity commands.

@par Compile-time dependencies

- <linux/joystick.h>

@par Provides

- @ref interface_joystick : joystick data
- @ref interface_position2d : joystick data represented as 2-D 
  position data.  Raw X- and Y-axis values are reported as xpos and ypos in the 
  position packet (all other fields are zero).

@par Requires

- @ref interface_position2d : if present, joystick positions will be
  interpreted as velocities and sent as commands to this position2d device.
  See also max_xspeed, max_yawspeed, and deadman_button options below.

@par Configuration requests

- None

@par Configuration file options

- port (string)
  - Default: "/dev/js0"
  - The joystick to be used.
- axes (integer tuple)
  - Default: [0 1]
  - Which joystick axes to call the "X" and "Y" axes, respectively.
- axis_maxima (integer tuple)
  - Default: [32767 32767]
  - Maximum absolute values attainable on the X and Y axes, respectively.
- axis_minima (integer tuple)
  - Default: [0 0]
  - Minimum values on the X and Y axes, respectively.  Anything smaller
    in absolute value than this limit will be reported as zero.
    Useful for implementing a dead zone on a touchy joystick.
- deadman_button (integer)
  - Default: -1
  - When controlling a @ref interface_position2d device, if deadman_button is 
    >= 0, this joystick button must be depressed for commands to be 
    sent to that device.
- max_xspeed (length / sec)
  - Default: 0.5 m/sec
  - The maximum absolute translational velocity to be used when commanding a
    position device.
- max_yawspeed (angle / sec)
  - Default: 30 deg/sec
  - The maximum absolute rotational velocity to be used when commanding a
    position device.
- timeout (float)
  - Default: 5.0
  - Time (in seconds) since receiving a new joystick event after which
    the underlying position device will be stopped, for safety.  Set to
    0.0 for no timeout.

@par Examples

Basic configuration

@verbatim
driver
(
  name "linuxjoystick"
  provides ["joystick:0"]
  port "/dev/js0"
)
@endverbatim

Provide a position interface, instead of a joystick interface.

@verbatim
driver
(
  name "linuxjoystick"
  provides ["position:0"]
  port "/dev/js0"
)
@endverbatim

Controlling a Pioneer, plus remapping joystick axes and setting various
limits.

@verbatim
driver
(
  name "p2os"
  provides ["odometry::position:0"]
  port "/dev/usb/tts/0"
)

driver
(
  name "linuxjoystick"
  provides ["joystick:0"]
  requires ["odometry::position:0"]
  max_yawspeed 50
  max_xspeed 0.5
  axes [3 4]
  axis_minima [5000 5000]
  port "/dev/js0"
  alwayson 1
)
@endverbatim

@todo
Add support for continuously sending commands, which might be needed for 
position devices that use watchdog timers.

@author Andrew Howard, Brian Gerkey, Paul Osmialowski

*/
/** @} */

#include <netinet/in.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <sys/time.h>
#include <fcntl.h>
#include <math.h>
#include <linux/joystick.h>

#include <replace/replace.h> // for poll(2)
#include <libplayercore/playercore.h>
#include <libplayercore/error.h>

extern PlayerTime *GlobalTime;

#define XAXIS 0
#define YAXIS 1

#define MAX_XSPEED 0.5
#define MAX_YAWSPEED DTOR(30.0)
#define AXIS_MAX ((int16_t) 32767)

////////////////////////////////////////////////////////////////////////////////
// The class for the driver
class LinuxJoystick : public Driver
{
  // Constructor; need that
  public: LinuxJoystick(ConfigFile* cf, int section);

  // Must implement the following methods.
  public: int Setup();
  public: int Shutdown();

  // Main function for device thread.
  private: virtual void Main();

  public: virtual int ProcessMessage(MessageQueue* resp_queue, 
                                     player_msghdr * hdr, 
                                     void * data) {return -1;}

  // Read the joystick
  private: void ReadJoy();

  // Write new data to server
  private: void RefreshData();

  // Check for new configuration requests
  //private: void CheckConfig();

  // Put new position command
  private: void PutPositionCommand();

  // Joystick device
  private: player_devaddr_t joystick_addr;
  private: const char *dev;
  private: int fd;
  private: int16_t xpos, ypos;
  private: uint16_t buttons;
  private: int xaxis_max, yaxis_max;
  private: int xaxis_min, yaxis_min;
  private: double timeout;
  private: struct timeval lastread;

  // Position device
  private: player_devaddr_t position_addr;
  private: player_position2d_data_t pos_data;

  // These are used when we send commands to a position device
  private: double max_xspeed, max_yawspeed;
  private: int xaxis, yaxis;
  private: int deadman_button;
  private: player_devaddr_t cmd_position_addr;
  private: Device* position;

  // Joystick
  private: player_joystick_data_t joy_data;
};


////////////////////////////////////////////////////////////////////////////////
// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class).  In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver* LinuxJoystick_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return ((Driver*) (new LinuxJoystick(cf, section)));
}


////////////////////////////////////////////////////////////////////////////////
// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void LinuxJoystick_Register(DriverTable* table)
{
  table->AddDriver("linuxjoystick", LinuxJoystick_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
LinuxJoystick::LinuxJoystick(ConfigFile* cf, int section) : Driver(cf, section)
{
  // zero ids, so that we'll know later which interfaces were requested
  memset(&this->cmd_position_addr, 0, sizeof(player_devaddr_t));
  memset(&this->position_addr, 0, sizeof(player_devaddr_t));
  memset(&this->joystick_addr, 0, sizeof(player_devaddr_t));

  // Do we create a position interface?
  if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides",
                        PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_addr))
    {
      this->SetError(-1);    
      return;
    }
  }
  // Do we create a joystick interface?
  if(cf->ReadDeviceAddr(&(this->joystick_addr), section, "provides",
                        PLAYER_JOYSTICK_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->joystick_addr))
    {
      this->SetError(-1);    
      return;
    }
  }

  this->dev = cf->ReadString(section, "port", "/dev/js0");
  this->xaxis = cf->ReadTupleInt(section,"axes", 0, XAXIS);