segwayrmp.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2003  John Sweeney & Brian Gerkey
 *
 *  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: Driver for the Segway RMP
  Author: John Sweeney and Brian Gerkey
  Date:
  CVS: $Id: segwayrmp.cc,v 1.40.2.2 2006/06/09 01:17:52 gerkey Exp $
*/

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_segwayrmp segwayrmp
 * @brief Segway RMP mobile robot

@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

The driver has been updated to the point that it compiles, but a few key requests
have been disabled and it has not been tested in anyway.

The segwayrmp driver provides control of a Segway RMP (Robotic
Mobility Platform), which is an experimental robotic version of the
Segway HT (Human Transport), a kind of two-wheeled, self-balancing
electric scooter.

@par Compile-time dependencies

- libcanlib (from Kvaser)

@par Notes

- Because of its power, weight, height, and dynamics, the Segway RMP is
a potentially dangerous machine.  Be @e very careful with it.

- Although the RMP does not actually support motor power control from 
software, for safety you must explicitly enable the motors using a
@p PLAYER_POSITION_MOTOR_POWER_REQ or @p PLAYER_POSITION3D_MOTOR_POWER_REQ
(depending on which interface you are using).  You must @e also enable
the motors in the command packet, by setting the @p state field to 1.

- For safety, this driver will stop the RMP (i.e., send zero velocities)
if no new command has been received from a client in the previous 400ms or so.
Thus, even if you want to continue moving at a constant velocity, you must
continuously send your desired velocities.

- Most of the configuration requests have not been tested.

- Currently, the only supported type of CAN I/O is "kvaser", which uses
Kvaser, Inc.'s CANLIB interface library.  This library provides access
to CAN cards made by Kvaser, such as the LAPcan II.  However, the CAN
I/O subsystem within this driver is modular, so that it should be pretty
straightforward to add support for other CAN cards.


@par Provides

- @ref interface_position2d
  - This interface returns odometry data, and accepts velocity commands.

- @ref interface_position3d
  - This interface returns odometry data (x, y and yaw) from the wheel
  encoders, and attitude data (pitch and roll) from the IMU.  The
  driver accepts velocity commands (x vel and yaw vel).

- @ref interface_power
  - Returns the current battery voltage (72 V when fully charged).

@par Configuration requests

- position interface
  - PLAYER_POSITION_POWER_REQ

- position3d interface
  - PLAYER_POSITION_POWER_REQ

@par Requires

- none

@par Configuration file options

- canio (string)
  - Default: "kvaser"
  - Type of CANbus driver.

- max_xspeed (length / sec)
  - Default: 0.5 m/s
  - Maximum linear speed

- max_yawspeed (angle / sec)
  - Default: 40 deg/sec
  - Maximum angular speed
      
@par Example 

@verbatim
driver
(
  name "segwayrmp"
  provides ["position:0" "position3d:0" "power:0"]
)
@endverbatim

@author John Sweeney, Brian Gerkey, Andrew Howard
*/
/** @} */

  
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <math.h>

#define PLAYER_ENABLE_MSG 0

#include "player.h"
#include "error.h"
#include "driver.h"
#include "drivertable.h"

#include "rmp_frame.h"
#include "segwayrmp.h"


// Number of RMP read cycles, without new speed commands from clients,
// after which we'll stop the robot (for safety).  The read loop
// seems to run at about 50Hz, or 20ms per cycle.
#define RMP_TIMEOUT_CYCLES 20 // about 400ms


////////////////////////////////////////////////////////////////////////////////
// A factory creation function
Driver* SegwayRMP_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return ((Driver*) (new SegwayRMP(cf, section)));
}


////////////////////////////////////////////////////////////////////////////////
// A driver registration function.
void SegwayRMP_Register(DriverTable* table)
{
  table->AddDriver("segwayrmp", SegwayRMP_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
SegwayRMP::SegwayRMP(ConfigFile* cf, int section)
    : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
  memset(&this->position_id, 0, sizeof(player_devaddr_t));
  memset(&this->position3d_id, 0, sizeof(player_devaddr_t));
  memset(&this->power_id, 0, sizeof(player_devaddr_t));

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

  // Do we create a position3d interface?
  if(cf->ReadDeviceAddr(&(this->position3d_id), section, "provides", 
                        PLAYER_POSITION3D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position3d_id) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // Do we create a power interface?
  if(cf->ReadDeviceAddr(&(this->power_id), section, "provides", 
                        PLAYER_POWER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->power_id) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  this->canio = NULL;
  this->caniotype = cf->ReadString(section, "canio", "kvaser");
  this->max_xspeed = (int) (1000 * cf->ReadLength(section, "max_xspeed", 0.5));
  if(this->max_xspeed < 0)
    this->max_xspeed = -this->max_xspeed;
  this->max_yawspeed = (int) (RTOD(cf->ReadAngle(section, "max_yawspeed", 40)));
  if(this->max_yawspeed < 0)
    this->max_yawspeed = -this->max_yawspeed;
  
  return;
}


SegwayRMP::~SegwayRMP()
{
  return;
}

int
SegwayRMP::Setup()
{
  // Clear the command buffers
#if 0
  if (this->position_id.code)
    ClearCommand(this->position_id);
  if (this->position3d_id.code)
    ClearCommand(this->position3d_id);
#endif

  PLAYER_MSG0(2, "CAN bus initializing");

  if(!strcmp(this->caniotype, "kvaser"))
    assert(this->canio = new CANIOKvaser);
  else
  {
    PLAYER_ERROR1("Unknown CAN I/O type: \"%s\"", this->caniotype);
    return(-1);
  }

  // start the CAN at 500 kpbs
  if(this->canio->Init(BAUD_500K) < 0) 
  {
    PLAYER_ERROR("error on CAN Init");
    return(-1);
  }

  // Initialize odometry
  this->odom_x = this->odom_y = this->odom_yaw = 0.0;

  this->curr_xspeed = this->curr_yawspeed = 0.0;
  this->motor_allow_enable = false;
  this->motor_enabled = false;
  this->firstread = true;
  this->timeout_counter = 0;

  this->StartThread();

  return(0);
}

int
SegwayRMP::Shutdown()
{
  PLAYER_MSG0(2, "Shutting down CAN bus");
  fflush(stdout);
  
  // TODO: segfaulting in here somewhere on client disconnect, but only 
  // sometimes.  
  //
  // UPDATE: This might have been fixed by moving the call to StopThread()
  // to before the sending of zero velocities.   There could have been
  // a race condition, since Shutdown() is called from the server's thread 
  // context.

  StopThread();
  
  // send zero velocities, for some semblance of safety
  CanPacket pkt;

  MakeVelocityCommand(&pkt,0,0);
  Write(pkt);

  // shutdown the CAN
  canio->Shutdown();
  delete canio;
  canio = NULL;
  
  return(0);
}

// Main function for device thread.
void 
SegwayRMP::Main()
{
  //unsigned char buffer[256];
  //size_t buffer_len;
  //player_position2d_cmd_t position_cmd;
  //player_position3d_cmd_t position3d_cmd;
  //void *client;
  CanPacket pkt;
  //int32_t xspeed,yawspeed;
  //bool got_command;
  bool first;

  pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

  first = true;  
  PLAYER_MSG0(2, "starting main loop");
  
  for(;;)
  {
    pthread_testcancel();
    
    // Read from the RMP
    if(Read() < 0)
    {
      PLAYER_ERROR("Read() errored; bailing");
      pthread_exit(NULL);
    }

    // Note that we got some data
    if (first)
    {
      first = false;
      PLAYER_MSG0(2, "got data from rmp");
    }

    // TODO: report better timestamps, possibly using time info from the RMP

    // Send data to clients
    Publish(this->position_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
              &this->position_data, sizeof(this->position_data), NULL);
    Publish(this->position3d_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE,
              &this->position3d_data, sizeof(this->position3d_data), NULL);
    Publish(this->power_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE,
              &this->power_data, sizeof(this->power_data), NULL);
    
/*    // check for config requests from the position interface
    if((buffer_len = GetConfig(this->position_id, &client, buffer, sizeof(buffer),NULL)) > 0)
    {
      // if we write to the CAN bus as a result of the config, don't write
      // a velocity command (may need to make this smarter if we get slow
      // velocity control).
      if(HandlePositionConfig(client,buffer,buffer_len) > 0)
        continue;
    }