p2os.cc

机器人仿真软件

      this->SetError(-1);
      return;
    }
  }

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

  // Do we create a limb interface?
  if(cf->ReadDeviceAddr(&(this->limb_id), section, "provides", PLAYER_LIMB_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->limb_id) != 0)
    {
      this->SetError(-1);
      return;
    }
    // If we do, we need a kinematics calculator
    kineCalc = new KineCalc;
  }
  else
    kineCalc = NULL;

  // Do we create an actarray interface? Note that if we have a limb interface, this implies an actarray interface
  if((cf->ReadDeviceAddr(&(this->actarray_id), section, "provides", PLAYER_ACTARRAY_CODE, -1, NULL) == 0) ||
      (this->limb_id.interf))
  {
    if(this->AddInterface(this->actarray_id) != 0)
    {
      this->SetError(-1);
      return;
    }
    // Stop actarray messages in the queue from being overwritten
    this->InQueue->AddReplaceRule (this->actarray_id, PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_POS_CMD, false);
    this->InQueue->AddReplaceRule (this->actarray_id, PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_SPEED_CMD, false);
    this->InQueue->AddReplaceRule (this->actarray_id, PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_HOME_CMD, false);
  }

  // build the table of robot parameters.
  ::initialize_robot_params();

  // Read config file options
  this->bumpstall = cf->ReadInt(section,"bumpstall",-1);
  this->pulse = cf->ReadFloat(section,"pulse",-1);
  this->rot_kp = cf->ReadInt(section, "rot_kp", -1);
  this->rot_kv = cf->ReadInt(section, "rot_kv", -1);
  this->rot_ki = cf->ReadInt(section, "rot_ki", -1);
  this->trans_kp = cf->ReadInt(section, "trans_kp", -1);
  this->trans_kv = cf->ReadInt(section, "trans_kv", -1);
  this->trans_ki = cf->ReadInt(section, "trans_ki", -1);

  this->psos_serial_port = cf->ReadString(section,"port",DEFAULT_P2OS_PORT);
  //this->psos_use_tcp = cf->ReadBool(section, "use_tcp", false); // TODO after ReadBool added
  this->psos_use_tcp = cf->ReadInt(section, "use_tcp", 0);
  this->psos_tcp_host = cf->ReadString(section, "tcp_remote_host", DEFAULT_P2OS_TCP_REMOTE_HOST);
  this->psos_tcp_port = cf->ReadInt(section, "tcp_remote_port", DEFAULT_P2OS_TCP_REMOTE_PORT);
  this->radio_modemp = cf->ReadInt(section, "radio", 0);
  this->joystickp = cf->ReadInt(section, "joystick", 0);
  this->direct_wheel_vel_control =
          cf->ReadInt(section, "direct_wheel_vel_control", 1);
  this->motor_max_speed = (int)rint(1e3 * cf->ReadLength(section,
                                                         "max_xspeed",
                                                         MOTOR_DEF_MAX_SPEED));
  this->motor_max_turnspeed = (int)rint(RTOD(cf->ReadAngle(section,
                                                         "max_yawspeed",
                                                         MOTOR_DEF_MAX_TURNSPEED)));
  this->motor_max_trans_accel = (short)rint(1e3 *
                                            cf->ReadLength(section,
                                                           "max_xaccel", 0));
  this->motor_max_trans_decel = (short)rint(1e3 *
                                            cf->ReadLength(section,
                                                           "max_xdecel", 0));
  this->motor_max_rot_accel = (short)rint(RTOD(cf->ReadAngle(section,
                                                             "max_yawaccel",
                                                             0)));
  this->motor_max_rot_decel = (short)rint(RTOD(cf->ReadAngle(section,
                                                             "max_yawdecel",
                                                             0)));

  this->use_vel_band = cf->ReadInt(section, "use_vel_band", 0);

  // Limb configuration
  if(kineCalc)
  {
    limb_data.state = PLAYER_LIMB_STATE_IDLE;
    armOffsetX = cf->ReadTupleFloat(section, "limb_pos", 0, 0.0f);
    armOffsetY = cf->ReadTupleFloat(section, "limb_pos", 1, 0.0f);
    armOffsetZ = cf->ReadTupleFloat(section, "limb_pos", 2, 0.0f);
    double temp1 = cf->ReadTupleFloat(section, "limb_links", 0, 0.06875f);
    double temp2 = cf->ReadTupleFloat(section, "limb_links", 1, 0.16f);
    double temp3 = cf->ReadTupleFloat(section, "limb_links", 2, 0.0f);
    double temp4 = cf->ReadTupleFloat(section, "limb_links", 3, 0.13775f);
    double temp5 = cf->ReadTupleFloat(section, "limb_links", 4, 0.11321f);
    kineCalc->SetLinkLengths (temp1, temp2, temp3, temp4, temp5);
    kineCalc->SetOffset (0, cf->ReadTupleFloat(section, "limb_offsets", 0, 0.0f));
    kineCalc->SetOffset (0, cf->ReadTupleFloat(section, "limb_offsets", 1, 0.0f));
    kineCalc->SetOffset (0, cf->ReadTupleFloat(section, "limb_offsets", 2, 0.0f));
    kineCalc->SetOffset (0, cf->ReadTupleFloat(section, "limb_offsets", 3, 0.0f));
    kineCalc->SetOffset (0, cf->ReadTupleFloat(section, "limb_offsets", 4, 0.0f));
  }

  this->psos_fd = -1;

  this->sent_gripper_cmd = false;
  this->last_actarray_cmd_was_pos = true;
  memset (&last_actarray_pos_cmd, 0, sizeof (player_actarray_position_cmd_t));
  memset (&last_actarray_home_cmd, 0, sizeof (player_actarray_home_cmd_t));
}

int P2OS::Setup()
{
  int i;
  // this is the order in which we'll try the possible baud rates. we try 9600
  // first because most robots use it, and because otherwise the radio modem
  // connection code might not work (i think that the radio modems operate at
  // 9600).
  int bauds[] = {B9600, B38400, B19200, B115200, B57600};
  int numbauds = sizeof(bauds);
  int currbaud = 0;

  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;


  if(this->psos_use_tcp)
  {

    // TCP socket:

    printf("P2OS connecting to remote host (%s:%d)... ", this->psos_tcp_host, this->psos_tcp_port);
    fflush(stdout);
    if( (this->psos_fd = socket(PF_INET, SOCK_STREAM, 0)) < 0)
    {
      perror("P2OS::Setup():socket():");
      return(1);
    }
    //printf("created socket %d.\nLooking up hostname...\n", this->psos_fd);
    struct hostent* h = gethostbyname(this->psos_tcp_host);
    if(!h)
    {
      perror("Error looking up hostname or address %s:");
      return(1);
    }
    struct sockaddr_in addr;
    assert(h->h_length <= (int) (sizeof(addr.sin_addr)));
    //printf("gethostbyname returned address %d length %d.\n", * h->h_addr, h->h_length);
    memcpy(&(addr.sin_addr), h->h_addr, h->h_length);
    //printf("copied address to addr.sin_addr.s_addr=%d\n", addr.sin_addr.s_addr);
    addr.sin_family = AF_INET;
    addr.sin_port = htons(this->psos_tcp_port);
    printf("Found host address, connecting...");
    fflush(stdout);
    if(connect(this->psos_fd, (struct sockaddr*) &addr, sizeof(addr)) < 0)
    {
      perror("Error Connecting to remote host (P2OS::Setup()::connect()):");
      return(1);
    }
    fcntl(this->psos_fd, F_SETFL, O_SYNC | O_NONBLOCK);
    if((flags = fcntl(this->psos_fd, F_GETFL)) < 0)
    {
      perror("P2OS::Setup():fcntl()");
      close(this->psos_fd);
      this->psos_fd = -1;
      return(1);
    }
    assert(flags & O_NONBLOCK);
    printf("TCP socket connection is OK... ");
    fflush(stdout);
  }
  else
  {

    // Serial port:

    printf("P2OS connection opening serial port %s...",this->psos_serial_port);
    fflush(stdout);

    if((this->psos_fd = open(this->psos_serial_port,
                     O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
    {
      perror("P2OS::Setup():open():");
      return(1);
    }

    if(tcgetattr( this->psos_fd, &term ) < 0 )
    {
      perror("P2OS::Setup():tcgetattr():");
      close(this->psos_fd);
      this->psos_fd = -1;
      return(1);
    }

    cfmakeraw( &term );
    cfsetispeed(&term, bauds[currbaud]);
    cfsetospeed(&term, bauds[currbaud]);


    if(tcsetattr(this->psos_fd, TCSAFLUSH, &term ) < 0)
    {
      perror("P2OS::Setup():tcsetattr():");
      close(this->psos_fd);
      this->psos_fd = -1;
      return(1);
    }

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

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

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

      usleep(50000);
      char modem_buf[50];
      int buf_len = read(this->psos_fd, modem_buf, 5);          // get "WMS2"
      modem_buf[buf_len]='\0';
      printf("wireless modem response = %s\n", modem_buf);

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

      // wait until get "Connected to address 2"
      int modem_connect_try = 10;
      while(strstr(modem_buf, "ected to addres") == NULL)
      {
        puts("Initializing radio modem...");
        write(this->psos_fd, "WMS2\r", 5);

        usleep(50000);
        char modem_buf[50];
        int buf_len = read(this->psos_fd, modem_buf, 5);          // get "WMS2"
        modem_buf[buf_len]='\0';
        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)
        {
          usleep(300000);
          buf_len = read(this->psos_fd, modem_buf, 40);
          modem_buf[buf_len]='\0';
          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)
          {
            puts("Failed to connect radio modem, Trying direct connection...");
            break;
          }
        }
      }
    }
    printf("Connected to robot device, handshaking with P2OS...");
    fflush(stdout);
  }// end TCP socket or serial port.

  // Sync:

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

    if(receivedpacket.Receive(this->psos_fd))
    {
      if((psos_state == NO_SYNC) && (num_sync_attempts >= 0))
      {
        num_sync_attempts--;
        usleep(P2OS_CYCLETIME_USEC);
        continue;
      }
      else
      {
        // couldn't connect; try different speed.
        if(++currbaud < numbauds)
        {
          cfsetispeed(&term, bauds[currbaud]);
          cfsetospeed(&term, bauds[currbaud]);
          if( tcsetattr(this->psos_fd, TCSAFLUSH, &term ) < 0 )
          {
            perror("P2OS::Setup():tcsetattr():");
            close(this->psos_fd);
            this->psos_fd = -1;
            return(1);
          }

          if(tcflush(this->psos_fd, TCIOFLUSH ) < 0 )
          {
            perror("P2OS::Setup():tcflush():");
            close(this->psos_fd);
            this->psos_fd = -1;
            return(1);
          }
          num_sync_attempts = 3;
          continue;
        }
        else
        {
          // tried all speeds; bail
          break;
        }
      }
    }

    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(this->psos_fd);
          sent_close = true;
          usleep(2*P2OS_CYCLETIME_USEC);
          tcflush(this->psos_fd,TCIFLUSH);