p2os.cc

机器人仿真软件

  // put compass data
  this->Publish(this->compass_id, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_POSITION2D_DATA_STATE,
                (void*)&(this->p2os_data.compass),
                sizeof(player_position2d_data_t),
                NULL);

  // put gyro data
  this->Publish(this->gyro_id, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_POSITION2D_DATA_STATE,
                (void*)&(this->p2os_data.gyro),
                sizeof(player_position2d_data_t),
                NULL);

  // put blobfinder data
  this->Publish(this->blobfinder_id, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_BLOBFINDER_DATA_BLOBS,
                (void*)&(this->p2os_data.blobfinder),
                sizeof(player_blobfinder_data_t),
                NULL);

  // put actarray data
  this->Publish(this->actarray_id, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_ACTARRAY_DATA_STATE,
                (void*)&(this->p2os_data.actarray),
                sizeof(player_actarray_data_t),
                NULL);

  // put limb data
  this->Publish(this->limb_id, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_LIMB_DATA,
                (void*)&(this->limb_data),
                sizeof(player_limb_data_t),
                NULL);
}

void
P2OS::Main()
{
  int last_sonar_subscrcount=0;
  int last_position_subscrcount=0;
  int last_actarray_subscrcount=0;
  double currentTime;
  struct timeval timeVal;

  for(;;)
  {
    pthread_testcancel();

    // we want to turn on the sonars if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    this->Lock();
    if(!last_sonar_subscrcount && this->sonar_subscriptions)
      this->ToggleSonarPower(1);
    else if(last_sonar_subscrcount && !(this->sonar_subscriptions))
      this->ToggleSonarPower(0);
    last_sonar_subscrcount = this->sonar_subscriptions;

    // Same for the actarray - this will also turn it on and off with limb subscriptions
    if(!last_actarray_subscrcount && this->actarray_subscriptions)
      this->ToggleActArrayPower(1, false);
    else if(last_actarray_subscrcount && !(this->actarray_subscriptions))
      this->ToggleActArrayPower(0, false);
    last_actarray_subscrcount = this->actarray_subscriptions;

    // we want to reset the odometry and enable the motors if the first
    // client just subscribed to the position device, and we want to stop
    // and disable the motors if the last client unsubscribed.
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      this->ToggleMotorPower(0);
      this->ResetRawPositions();
    }
    else if(last_position_subscrcount && !(this->position_subscriptions))
    {
      // enable motor power
      this->ToggleMotorPower(1);
    }
    last_position_subscrcount = this->position_subscriptions;
    this->Unlock();

    // The Amigo board seems to drop commands once in a while.  This is
    // a hack to restart the serial reads if that happens.
    if(this->blobfinder_id.interf)
    {
      struct timeval now_tv;
      GlobalTime->GetTime(&now_tv);
      if (now_tv.tv_sec > lastblob_tv.tv_sec)
      {
        P2OSPacket cam_packet;
        unsigned char cam_command[4];

        cam_command[0] = GETAUX2;
        cam_command[1] = ARGINT;
        cam_command[2] = 0;
        cam_command[3] = 0;
        cam_packet.Build(cam_command, 4);
        SendReceive(&cam_packet);

        cam_command[0] = GETAUX2;
        cam_command[1] = ARGINT;
        cam_command[2] = CMUCAM_MESSAGE_LEN * 2 -1;
        cam_command[3] = 0;
        cam_packet.Build(cam_command, 4);
        SendReceive(&cam_packet);
        GlobalTime->GetTime(&lastblob_tv);  // Reset last blob packet time
      }
    }

    // handle pending messages
    if(!this->InQueue->Empty())
    {
      ProcessMessages();
    }

    // Check if need to send a pulse to the robot
    if (this->pulse != -1)
    {
      gettimeofday (&timeVal, NULL);
      currentTime = static_cast<double> (timeVal.tv_sec) + (static_cast<double> (timeVal.tv_usec) / 1e6);
      if ((currentTime - lastPulseTime) > this->pulse)
      {
        SendPulse ();
        // Update the time of last pulse/command
        lastPulseTime = currentTime;
      }
    }
    else
    {
      // Hack fix to get around the fact that if no commands are sent to the robot via SendReceive,
      // the driver will never read SIP packets and so never send data back to clients.
      // We need a better way of doing regular checks of the serial port - peek in sendreceive, maybe?
      // Because if there is no data waiting this will sit around waiting until one comes
      SendReceive (NULL, true);
    }
  }
}

/* send the packet, then receive and parse an SIP */
int
P2OS::SendReceive(P2OSPacket* pkt, bool publish_data)
{
  P2OSPacket packet;

  // zero the combined data buffer.  it will be filled with the latest data
  // by SIP::Fill()
  memset(&(this->p2os_data),0,sizeof(player_p2os_data_t));
  if((this->psos_fd >= 0) && this->sippacket)
  {
    if(pkt)
      pkt->Send(this->psos_fd);

    /* receive a packet */
    pthread_testcancel();
    if(packet.Receive(this->psos_fd))
    {
      puts("RunPsosThread(): Receive errored");
      pthread_exit(NULL);
    }

    if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
       (packet.packet[3] == 0x30 || packet.packet[3] == 0x31) ||
       (packet.packet[3] == 0x32 || packet.packet[3] == 0x33) ||
       (packet.packet[3] == 0x34))
    {

      /* It is a server packet, so process it */
      this->sippacket->Parse( &packet.packet[3] );
      this->sippacket->Fill(&(this->p2os_data));

      if(publish_data)
        this->PutData();
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
            packet.packet[3] == SERAUX)
    {
       // This is an AUX serial packet
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
            packet.packet[3] == SERAUX2)
    {
      // This is an AUX2 serial packet
      if(blobfinder_id.interf)
      {
        /* It is an extended SIP (blobfinder) packet, so process it */
        /* Be sure to pass data size too (packet[2])! */
        this->sippacket->ParseSERAUX( &packet.packet[2] );
        this->sippacket->Fill(&(this->p2os_data));

        if(publish_data)
          this->PutData();

        P2OSPacket cam_packet;
        unsigned char cam_command[4];

        /* We cant get the entire contents of the buffer,
        ** and we cant just have P2OS send us the buffer on a regular basis.
        ** My solution is to flush the buffer and then request exactly
        ** CMUCAM_MESSAGE_LEN * 2 -1 bytes of data.  This ensures that
        ** we will get exactly one full message, and it will be "current"
        ** within the last 2 messages.  Downside is that we end up pitching
        ** every other CMUCAM message.  Tradeoffs... */
        // Flush
        cam_command[0] = GETAUX2;
        cam_command[1] = ARGINT;
        cam_command[2] = 0;
        cam_command[3] = 0;
        cam_packet.Build(cam_command, 4);
        this->SendReceive(&cam_packet,publish_data);

        // Reqest next packet
        cam_command[0] = GETAUX2;
        cam_command[1] = ARGINT;
        // Guarantee exactly 1 full message
        cam_command[2] = CMUCAM_MESSAGE_LEN * 2 -1;
        cam_command[3] = 0;
        cam_packet.Build(cam_command, 4);
        this->SendReceive(&cam_packet,publish_data);
        GlobalTime->GetTime(&lastblob_tv);  // Reset last blob packet time
      }
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
            (packet.packet[3] == 0x50 || packet.packet[3] == 0x80) ||
//            (packet.packet[3] == 0xB0 || packet.packet[3] == 0xC0) ||
            (packet.packet[3] == 0xC0) ||
            (packet.packet[3] == 0xD0 || packet.packet[3] == 0xE0))
    {
      /* It is a vision packet from the old Cognachrome system*/

      /* we don't understand these yet, so ignore */
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
            packet.packet[3] == GYROPAC)
    {
      if(this->gyro_id.interf)
      {
        /* It's a set of gyro measurements */
        this->sippacket->ParseGyro(&packet.packet[2]);
        this->sippacket->Fill(&(this->p2os_data));
        if(publish_data)
          this->PutData();

        /* Now, the manual says that we get one gyro packet each cycle,
         * right before the standard SIP.  So, we'll call SendReceive()
         * again (with no packet to send) to get the standard SIP.  There's
         * a definite danger of infinite recursion here if the manual
         * is wrong.
         */
        this->SendReceive(NULL,publish_data);
      }
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
            (packet.packet[3] == 0x20))
    {
      //printf("got a CONFIGpac:%d\n",packet.size);
    }
    else if (packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && packet.packet[3] == ARMPAC)
    {
      if (actarray_id.interf)
      {
        // ARMpac - current arm status
        double joints[6];
        sippacket->ParseArm (&packet.packet[2]);
        for (int ii = 0; ii < 6; ii++)
        {
          sippacket->armJointPosRads[ii] = TicksToRadians (ii, sippacket->armJointPos[ii]);
          joints[ii] = sippacket->armJointPosRads[ii];
        }
        sippacket->Fill(&p2os_data);
        if(kineCalc)
        {
          kineCalc->CalculateFK (joints);
          limb_data.position.px = kineCalc->GetP ().x + armOffsetX;
          limb_data.position.py = -kineCalc->GetP ().y + armOffsetY;
          limb_data.position.pz = kineCalc->GetP ().z + armOffsetZ;
          limb_data.approach.px = kineCalc->GetA ().x;
          limb_data.approach.py = -kineCalc->GetA ().y;
          limb_data.approach.pz = kineCalc->GetA ().z;
          limb_data.orientation.px = kineCalc->GetO ().x;
          limb_data.orientation.py = -kineCalc->GetO ().y;
          limb_data.orientation.pz = kineCalc->GetO ().z;
          if (limb_data.state != PLAYER_LIMB_STATE_OOR && limb_data.state != PLAYER_LIMB_STATE_COLL)
          {
            if (sippacket->armJointMoving[0] || sippacket->armJointMoving[1] || sippacket->armJointMoving[2] ||
                sippacket->armJointMoving[3] || sippacket->armJointMoving[4])
            {
              limb_data.state = PLAYER_LIMB_STATE_MOVING;
            }
            else
              limb_data.state = PLAYER_LIMB_STATE_IDLE;
          }
        }
      }
      if(publish_data)
        this->PutData();
      // Go for another SIP - there had better be one or things will probably go boom
      SendReceive(NULL,publish_data);
    }
    else if (packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && packet.packet[3] == ARMINFOPAC)
    {
      // ARMINFOpac - arm configuration stuff
      if (actarray_id.interf)
      {
        sippacket->ParseArmInfo (&packet.packet[2]);
        // Update the KineCalc with the new info for joints - one would assume this doesn't change, though...
        if (kineCalc)
        {
          for (int ii = 0; ii < 5; ii++)
            kineCalc->SetJointRange (ii, TicksToRadians (ii, sippacket->armJoints[ii].min), TicksToRadians (ii, sippacket->armJoints[ii].max));
          // Go for another SIP - there had better be one or things will probably go boom
        }
        SendReceive(NULL,publish_data);
      }
    }
    else
    {
      packet.PrintHex();
    }
  }
  return(0);
}

void
P2OS::ResetRawPositions()
{
  P2OSPacket pkt;
  unsigned char p2oscommand[4];

  if(this->sippacket)
  {
    this->sippacket->rawxpos = 0;
    this->sippacket->rawypos = 0;
    this->sippacket->xpos = 0;
    this->sippacket->ypos = 0;
    p2oscommand[0] = SETO;
    p2oscommand[1] = ARGINT;
    pkt.Build(p2oscommand, 2);
    this->SendReceive(&pkt,false);
  }
}


/****************************************************************
** Reset the CMUcam.  This includes flushing the buffer and
** setting interface output mode to raw.  It also restarts
** tracking output (current mode)
****************************************************************/
void P2OS::CMUcamReset(bool doLock)
{
  CMUcamStopTracking(doLock); // Stop the current tracking.

  P2OSPacket cam_packet;
  unsigned char cam_command[8];

  printf("Resetting the CMUcam...\n");
  cam_command[0] = TTY3;
  cam_command[1] = ARGSTR;
  sprintf((char*)&cam_command[3], "RS\r");
  cam_command[2] = strlen((char *)&cam_command[3]);
  cam_packet.Build(cam_command, (int)cam_command[2]+3);
  this->SendReceive(&cam_packet,doLock);

  // Set for raw output + no ACK/NACK
  printf("Setting raw mode...\n");
  cam_command[0] = TTY3;
  cam_command[1] = ARGSTR;
  sprintf((char*)&cam_command[3], "RM 3\r");
  cam_command[2] = strlen((char *)&cam_command[3]);
  cam_packet.Build(cam_command, (int)cam_command[2]+3);
  this->SendReceive(&cam_packet,doLock);
  usleep(100000);

  printf("Flushing serial buffer...\n");
  cam_command[0] = GETAUX2;
  cam_command[1] = ARGINT;
  cam_command[2] = 0;
  cam_command[3] = 0;
  cam_packet.Build(cam_command, 4);
  this->SendReceive(&cam_packet,doLock);

  sleep(1);
  // (Re)start tracking
  this->CMUcamStartTracking(false);
}


/****************************************************************
** Start CMUcam blob tracking.  This method can be called 3 ways:
**   1) with a set of 6 color arguments (RGB min and max)
**   2) with auto tracking (-1 argument)
**   3) with current values (0 or no arguments)
****************************************************************/
void P2OS::CMUcamTrack(int rmin, int rmax,
                       int gmin, int gmax,
                       int bmin, int bmax)
{
  this->CMUcamStopTracking(); // Stop the current tracking.

  P2OSPacket cam_packet;
  unsigned char cam_command[50];

  if (!rmin && !rmax && !gmin && !gmax && !bmin && !bmax)
  {
    CMUcamStartTracking();
  }
  else if (rmin<0 || rmax<0 || gmin<0 || gmax<0 || bmin<0 || bmax<0)
  {
    printf("Activating CMUcam color tracking (AUTO-mode)...\n");
    cam_command[0] = TTY3;
    cam_command[1] = ARGSTR;
    sprintf((char*)&cam_command[3], "TW\r");
    cam_command[2] = strlen((char *)&cam_command[3]);