p2os.cc

机器人仿真平台,和stage配合运行

/*
 *  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
 *
 */

/*
 * $Id: p2os.cc,v 1.2.2.5 2003/05/25 00:59:25 gerkey Exp $
 *
 *   the P2OS device.  it's the parent device for all the P2 'sub-devices',
 *   like gripper, position, sonar, etc.  there's a thread here that
 *   actually interacts with P2OS via the serial line.  the other
 *   "devices" communicate with this thread by putting into and getting
 *   data out of shared buffers.
 */

#if HAVE_CONFIG_H
  #include <config.h>
#endif
#if HAVE_STRINGS_H
  #include <strings.h>
#endif


#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 <p2os.h>
#include <packet.h>

#include <playertime.h>
extern PlayerTime* GlobalTime;

// so we can access the deviceTable and extract pointers to the sonar
// and position objects
#include <devicetable.h>
extern CDeviceTable* deviceTable;
extern int global_playerport; // used to get at devices

/* here we calculate our conversion factors.
 *  0x370 is max value for the PTZ pan command, and in the real
 *    world, it has +- 25.0 range.
 *  0x12C is max value for the PTZ tilt command, and in the real
 *    world, it has +- 100.0 range.
 */
#define PTZ_PAN_MAX 100.0
#define PTZ_TILT_MAX 25.0
#define PTZ_PAN_CONV_FACTOR (0x370 / PTZ_PAN_MAX)
#define PTZ_TILT_CONV_FACTOR (0x12C / PTZ_TILT_MAX)

/* these are from personal experience */
#define MOTOR_MAX_SPEED 500
#define MOTOR_MAX_TURNRATE 100

/* these are necessary to make the static fields visible to the linker */
extern pthread_t       P2OS::thread;
extern struct timeval  P2OS::timeBegan_tv;
extern bool            P2OS::direct_wheel_vel_control;
extern int             P2OS::psos_fd; 
extern char            P2OS::psos_serial_port[];
extern int             P2OS::radio_modemp;
extern int             P2OS::joystickp;
extern bool            P2OS::initdone;
extern char            P2OS::num_loops_since_rvel;
extern SIP*           P2OS::sippacket;
extern int             P2OS::param_idx;
extern pthread_mutex_t P2OS::p2os_accessMutex;
extern pthread_mutex_t P2OS::p2os_setupMutex;
extern int             P2OS::p2os_subscriptions;
extern player_p2os_data_t*  P2OS::data;
extern player_p2os_cmd_t*  P2OS::command;
extern unsigned char*    P2OS::reqqueue;
extern unsigned char*    P2OS::repqueue;


P2OS::P2OS(char* interface, ConfigFile* cf, int section)
{
  int reqqueuelen = 1;
  int repqueuelen = 1;

  if(!initdone)
  {
    // build the table of robot parameters.
    initialize_robot_params();
  
    // also, install default parameter values.
    strncpy(psos_serial_port,DEFAULT_P2OS_PORT,sizeof(psos_serial_port));
    psos_fd = -1;
    radio_modemp = 0;
    joystickp = 0;
  
    data = new player_p2os_data_t;
    command = new player_p2os_cmd_t;

    reqqueue = (unsigned char*)(new playerqueue_elt_t[reqqueuelen]);
    repqueue = (unsigned char*)(new playerqueue_elt_t[repqueuelen]);

    SetupBuffers((unsigned char*)data, sizeof(player_p2os_data_t),
                 (unsigned char*)command, sizeof(player_p2os_cmd_t),
                 reqqueue, reqqueuelen,
                 repqueue, repqueuelen);

    ((player_p2os_cmd_t*)device_command)->position.xspeed = 0;
    ((player_p2os_cmd_t*)device_command)->position.yawspeed = 0;

    ((player_p2os_cmd_t*)device_command)->gripper.cmd = GRIPstore;
    ((player_p2os_cmd_t*)device_command)->gripper.arg = 0x00;

    p2os_subscriptions = 0;

    pthread_mutex_init(&p2os_accessMutex,NULL);
    pthread_mutex_init(&p2os_setupMutex,NULL);

    initdone = true; 
  }
  else
  {
    // every sub-device gets its own queue object (but they all point to the
    // same chunk of memory)
    
    // every sub-device needs to get its various pointers set up
    SetupBuffers((unsigned char*)data, sizeof(player_p2os_data_t),
                 (unsigned char*)command, sizeof(player_p2os_cmd_t),
                 reqqueue, reqqueuelen,
                 repqueue, repqueuelen);
  }



  strncpy(psos_serial_port,
          cf->ReadString(section, "port", psos_serial_port),
          sizeof(psos_serial_port));
  radio_modemp = cf->ReadInt(section, "radio", radio_modemp);
  joystickp = cf->ReadInt(section, "joystick", joystickp);

  // zero the subscription counter.
  subscriptions = 0;
}

P2OS::~P2OS()
{
  if(reqqueue)
  {
    delete[] reqqueue;
    reqqueue=NULL;
  }
  if(repqueue)
  {
    delete[] repqueue;
    repqueue=NULL;
  }
  if(data)
  {
    delete data;
    data = NULL;
  }
  if(command)
  {
    delete command;
    command = NULL;
  }
}

void P2OS::Lock()
{
  pthread_mutex_lock(&p2os_accessMutex);
}
void P2OS::Unlock()
{
  pthread_mutex_unlock(&p2os_accessMutex);
}

int P2OS::Setup()
{
  unsigned char sonarcommand[4];
  P2OSPacket sonarpacket; 
  int i;

  struct termios term;
  unsigned char command;
  P2OSPacket packet, receivedpacket;
  int flags;
  bool sent_close = false;
  enum
  {
    NO_SYNC,
    AFTER_FIRST_SYNC,
    AFTER_SECOND_SYNC,
    READY
  } psos_state;
    
  psos_state = NO_SYNC;

  char name[20], type[20], subtype[20];
  int cnt;

  printf("P2OS connection initializing (%s)...",psos_serial_port);
  fflush(stdout);

  if((psos_fd = open(psos_serial_port, 
                     O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
  {
    perror("P2OS::Setup():open():");
    return(1);
  }  
 
  if( tcgetattr( psos_fd, &term ) < 0 )
  {
    perror("P2OS::Setup():tcgetattr():");
    close(psos_fd);
    psos_fd = -1;
    return(1);
  }
  
#if HAVE_CFMAKERAW
  cfmakeraw( &term );
#endif
  cfsetispeed( &term, B9600 );
  cfsetospeed( &term, B9600 );
  
  if( tcsetattr( psos_fd, TCSAFLUSH, &term ) < 0 )
  {
    perror("P2OS::Setup():tcsetattr():");
    close(psos_fd);
    psos_fd = -1;
    return(1);
  }

  if( tcflush( psos_fd, TCIOFLUSH ) < 0 )
  {
    perror("P2OS::Setup():tcflush():");
    close(psos_fd);
    psos_fd = -1;
    return(1);
  }

  if((flags = fcntl(psos_fd, F_GETFL)) < 0)
  {
    perror("P2OS::Setup():fcntl()");
    close(psos_fd);
    psos_fd = -1;
    return(1);
  }

  // radio modem initialization code, courtesy of Kim Jinsuck 
  //   <[e-mail]>
  if(radio_modemp)
  {
    puts("Initializing radio modem...");
    write(psos_fd, "WMS2\r", 5);

    usleep(50000);
    char modem_buf[40];
    read(psos_fd, modem_buf, 5);          // get "WMS2"
    printf("wireless modem response = %s\n", modem_buf);

    usleep(10000);
    // get "\n\rConnecting..." --> \n\r is my guess
    read(psos_fd, modem_buf, 14); 
    printf("wireless modem response = %s\n", modem_buf);

    // wait until get "Connected to address 2"
    int modem_connect_try = 10;
    while (modem_buf[12] != 't') 
    {
      usleep(300000);
      read(psos_fd, modem_buf, 40);
      printf("wireless modem response = %s\n", modem_buf);
      // if "Partner busy!"
      if(modem_buf[2] == 'P') 
      {
        printf("Please reset partner modem and try again\n");
        return(1);
      }
      // if "\n\rPartner not found!"
      if(modem_buf[0] == 'P') 
      {
        printf("Please check partner modem and try again\n");
        return(1);
      }
      if(modem_connect_try-- == 0) 
      {
        printf("Failed to connect radio modem\n");
        return(1);
      }
    }
  }

  int num_sync_attempts = 5;
  while(psos_state != READY)
  {
    //printf("psos_state: %d\n", psos_state);
    switch(psos_state)
    {
      case NO_SYNC:
        command = SYNC0;
        packet.Build( &command, 1);
        packet.Send( psos_fd );
        usleep(P2OS_CYCLETIME_USEC);
        break;
      case AFTER_FIRST_SYNC:
        if(fcntl(psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
        {
          perror("P2OS::Setup():fcntl()");
          close(psos_fd);
          psos_fd = -1;
          return(1);
        }
        command = SYNC1;
        packet.Build( &command, 1);
        packet.Send( psos_fd );
        break;
      case AFTER_SECOND_SYNC:
        command = SYNC2;
        packet.Build( &command, 1);
        packet.Send( psos_fd );
        break;
      default:
        puts("P2OS::Setup():shouldn't be here...");
        break;
    }
    usleep(P2OS_CYCLETIME_USEC);

    if(receivedpacket.Receive( psos_fd ))
    {
      if((psos_state == NO_SYNC) && (num_sync_attempts >= 0))
      {
        num_sync_attempts--;
        usleep(P2OS_CYCLETIME_USEC);
        continue;
      }
      else
      {
        printf("Couldn't synchronize with P2OS.\n"  
               "  Most likely because the robot is not connected to %s\n", 
               psos_serial_port);
        close(psos_fd);
        psos_fd = -1;
        return(1);
      }
    }
    //receivedpacket.PrintHex();
    switch(receivedpacket.packet[3])
    {
      case SYNC0:
        psos_state = AFTER_FIRST_SYNC;
        break;
      case SYNC1:
        psos_state = AFTER_SECOND_SYNC;
        break;
      case SYNC2:
        psos_state = READY;
        break;
      default:
        // maybe P2OS is still running from last time.  let's try to CLOSE 
        // and reconnect
        if(!sent_close)
        {
          //puts("sending CLOSE");
          command = CLOSE;
          packet.Build( &command, 1);
          packet.Send( psos_fd );
          sent_close = true;
          usleep(2*P2OS_CYCLETIME_USEC);
          tcflush(psos_fd,TCIFLUSH);
          psos_state = NO_SYNC;
        }
        break;
    }
    usleep(P2OS_CYCLETIME_USEC);
  }

  cnt = 4;
  cnt += sprintf(name, "%s", &receivedpacket.packet[cnt]);
  cnt++;
  cnt += sprintf(type, "%s", &receivedpacket.packet[cnt]);
  cnt++;
  cnt += sprintf(subtype, "%s", &receivedpacket.packet[cnt]);
  cnt++;


  command = OPEN;
  packet.Build( &command, 1);
  packet.Send( psos_fd );
  usleep(P2OS_CYCLETIME_USEC);

  command = PULSE;
  packet.Build( &command, 1);
  packet.Send( psos_fd );
  usleep(P2OS_CYCLETIME_USEC);

  printf("Done.\n   Connected to %s, a %s %s\n", name, type, subtype);

  // now, based on robot type, find the right set of parameters
  for(i=0;i<PLAYER_NUM_ROBOT_TYPES;i++)
  {
    if(!strcasecmp(PlayerRobotParams[i].Class,type) && 
       !strcasecmp(PlayerRobotParams[i].Subclass,subtype))
    {
      param_idx = i;
      break;
    }