p2os.cc

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

  }
  if(i == PLAYER_NUM_ROBOT_TYPES)
  {
    fputs("P2OS: Warning: couldn't find parameters for this robot; "
            "using defaults\n",stderr);
    param_idx = 0;
  }
  
  direct_wheel_vel_control = true;
  num_loops_since_rvel = 2;
  //pthread_mutex_unlock(&serial_mutex);

  // first, receive a packet so we know we're connected.
  if(!sippacket)
    sippacket = new SIP(param_idx);
  SendReceive((P2OSPacket*)NULL);//,false);

  // turn off the sonars at first
  sonarcommand[0] = SONAR;
  sonarcommand[1] = 0x3B;
  sonarcommand[2] = 0;
  sonarcommand[3] = 0;
  sonarpacket.Build(sonarcommand, 4);
  SendReceive(&sonarpacket);

  if(joystickp)
  {
    // enable joystick control
    P2OSPacket js_packet;
    unsigned char js_command[4];
    js_command[0] = JOYDRIVE;
    js_command[1] = 0x3B;
    js_command[2] = 1;
    js_command[3] = 0;
    js_packet.Build(js_command, 4);
    SendReceive(&js_packet);
  }

  /* now spawn reading thread */
  StartThread();
  return(0);
}

int P2OS::Shutdown()
{
  unsigned char command[20],buffer[20];
  P2OSPacket packet; 

  memset(buffer,0,20);

  if(psos_fd == -1)
  {
    return(0);
  }

  StopThread();

  command[0] = STOP;
  packet.Build(command, 1);
  packet.Send(psos_fd);
  usleep(P2OS_CYCLETIME_USEC);

  command[0] = CLOSE;
  packet.Build( command, 1);
  packet.Send( psos_fd );
  usleep(P2OS_CYCLETIME_USEC);

  close(psos_fd);
  psos_fd = -1;
  puts("P2OS has been shutdown");
  delete sippacket;
  sippacket = NULL;

  return(0);
}

int P2OS::Subscribe(void *client)
{
  int setupResult;

  if(p2os_subscriptions == 0) 
  {
    setupResult = Setup();
    if (setupResult == 0 ) 
    {
      p2os_subscriptions++;  // increment the static p2os-wide subscr counter
      subscriptions++;       // increment the per-device subscr counter
    }
  }
  else 
  {
    p2os_subscriptions++;  // increment the static p2os-wide subscr counter
    subscriptions++;       // increment the per-device subscr counter
    setupResult = 0;
  }
  
  return( setupResult );
}

int P2OS::Unsubscribe(void *client)
{
  int shutdownResult;

  if(p2os_subscriptions == 0) 
  {
    shutdownResult = -1;
  }
  else if(p2os_subscriptions == 1) 
  {
    shutdownResult = Shutdown();
    if (shutdownResult == 0 ) 
    { 
      p2os_subscriptions--;  // decrement the static p2os-wide subscr counter
      subscriptions--;       // decrement the per-device subscr counter
    }
    /* do we want to unsubscribe even though the shutdown went bad? */
  }
  else 
  {
    p2os_subscriptions--;  // decrement the static p2os-wide subscr counter
    subscriptions--;       // decrement the per-device subscr counter
    shutdownResult = 0;
  }
  
  return( shutdownResult );
}


void P2OS::PutData( unsigned char* src, size_t maxsize,
                         uint32_t timestamp_sec, uint32_t timestamp_usec)
{
  Lock();

  *((player_p2os_data_t*)device_data) = *((player_p2os_data_t*)src);

  if(timestamp_sec == 0)
  {
    struct timeval curr;
    GlobalTime->GetTime(&curr);
    timestamp_sec = curr.tv_sec;
    timestamp_usec = curr.tv_usec;
  }

  data_timestamp_sec = timestamp_sec;
  data_timestamp_usec = timestamp_usec;
  
  // need to fill in the timestamps on all P2OS devices, both so that they
  // can read it, but also because other devices may want to read it
  player_device_id_t id = device_id;

  id.code = PLAYER_SONAR_CODE;
  CDevice* sonarp = deviceTable->GetDevice(id);
  if(sonarp)
  {
    sonarp->data_timestamp_sec = this->data_timestamp_sec;
    sonarp->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_POWER_CODE;
  CDevice* powerp = deviceTable->GetDevice(id);
  if(powerp)
  {
    powerp->data_timestamp_sec = this->data_timestamp_sec;
    powerp->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_BUMPER_CODE;
  CDevice* bumperp = deviceTable->GetDevice(id);
  if(bumperp)
  {
    bumperp->data_timestamp_sec = this->data_timestamp_sec;
    bumperp->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_AIO_CODE;
  CDevice* aio = deviceTable->GetDevice(id);
  if(aio)
  {
    aio->data_timestamp_sec = this->data_timestamp_sec;
    aio->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_DIO_CODE;
  CDevice* dio = deviceTable->GetDevice(id);
  if(dio)
  {
    dio->data_timestamp_sec = this->data_timestamp_sec;
    dio->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_POSITION_CODE;
  CDevice* positionp = deviceTable->GetDevice(id);
  if(positionp)
  {
    positionp->data_timestamp_sec = this->data_timestamp_sec;
    positionp->data_timestamp_usec = this->data_timestamp_usec;
  }

  id.code = PLAYER_GRIPPER_CODE;
  CDevice* gripperp = deviceTable->GetDevice(id);
  if(gripperp)
  {
    gripperp->data_timestamp_sec = this->data_timestamp_sec;
    gripperp->data_timestamp_usec = this->data_timestamp_usec;
  }

  Unlock();
}

void 
P2OS::Main()
{
  player_p2os_cmd_t command;
  unsigned char config[P2OS_CONFIG_BUFFER_SIZE];
  unsigned char motorcommand[4];
  unsigned char gripcommand[4];
  unsigned char soundcommand[4];
  P2OSPacket motorpacket; 
  P2OSPacket grippacket;
  P2OSPacket soundpacket;
  
  short speedDemand=0, turnRateDemand=0;
  char gripperCmd=0,gripperArg=0;
  bool newmotorspeed, newmotorturn;
  bool newgrippercommand;
  bool newsoundplay;
  unsigned short soundindex=0;
  
  double leftvel, rightvel;
  double rotational_term;
  unsigned short absspeedDemand, absturnRateDemand;

  int config_size;

  int last_sonar_subscrcount;
  int last_position_subscrcount;

  player_device_id_t id;
  id.port = global_playerport;
  id.index = 0;

  id.code = PLAYER_SONAR_CODE;
  CDevice* sonarp = deviceTable->GetDevice(id);
  id.code = PLAYER_POSITION_CODE;
  CDevice* positionp = deviceTable->GetDevice(id);
  id.code = PLAYER_SOUND_CODE;
  CDevice* soundp = deviceTable->GetDevice(id);
  
  last_sonar_subscrcount = 0;
  last_position_subscrcount = 0;

  if(sippacket)
    sippacket->x_offset = sippacket->y_offset = sippacket->angle_offset = 0;

  GlobalTime->GetTime(&timeBegan_tv);

  // request the current configuration
  /*
    configreq[0] = 18;
    configreq[1] = 0x3B;
    configpacket.Build(configreq,2);
    puts("sending CONFIGpac");
    SendReceive(&configpacket);
  */

  for(;;)
  {
    // we want to turn on the sonars if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    if(sonarp)
    {
      if(!last_sonar_subscrcount && sonarp->subscriptions)
      {
        motorcommand[0] = SONAR;
        motorcommand[1] = 0x3B;
        motorcommand[2] = 1;
        motorcommand[3] = 0;
        motorpacket.Build(motorcommand, 4);
        SendReceive(&motorpacket);
      }
      else if(last_sonar_subscrcount && !(sonarp->subscriptions))
      {
        motorcommand[0] = SONAR;
        motorcommand[1] = 0x3B;
        motorcommand[2] = 0;
        motorcommand[3] = 0;
        motorpacket.Build(motorcommand, 4);
        SendReceive(&motorpacket);
      }
      
      last_sonar_subscrcount = sonarp->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(positionp)
    {
      if(!last_position_subscrcount && positionp->subscriptions)
      {
        // disable motor power
        motorcommand[0] = ENABLE;
        motorcommand[1] = 0x3B;
        motorcommand[2] = 0;
        motorcommand[3] = 0;
        motorpacket.Build(motorcommand, 4);
        SendReceive(&motorpacket);//,false);

        // reset odometry
        ResetRawPositions();
      }
      else if(last_position_subscrcount && !(positionp->subscriptions))
      {
        // command motors to stop
        motorcommand[0] = VEL2;
        motorcommand[1] = 0x3B;
        motorcommand[2] = 0;
        motorcommand[3] = 0;

        // overwrite existing motor commands to be zero
        player_position_cmd_t position_cmd;
        position_cmd.xspeed = 0;
        position_cmd.yawspeed = 0;
        // TODO: who should really be the client here?
        positionp->PutCommand(this,(unsigned char*)(&position_cmd), 
                              sizeof(position_cmd));
      
        // disable motor power
        motorcommand[0] = ENABLE;
        motorcommand[1] = 0x3B;
        motorcommand[2] = 0;
        motorcommand[3] = 0;
        motorpacket.Build(motorcommand, 4);
        SendReceive(&motorpacket);//,false);
      }

      last_position_subscrcount = positionp->subscriptions;
    }

    
    void* client;
    player_device_id_t id;
    // first, check if there is a new config command
    if((config_size = GetConfig(&id, &client, (void*)config, sizeof(config))))
    {
      switch(id.code)
      {
        case PLAYER_SONAR_CODE:
          switch(config[0])
          {
            case PLAYER_SONAR_POWER_REQ:
              /*
               * 1 = enable sonars
               * 0 = disable sonar
               */
              if(config_size != sizeof(player_sonar_power_config_t))
              {
                puts("Arg to sonar state change request wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }
              player_sonar_power_config_t sonar_config;
              sonar_config = *((player_sonar_power_config_t*)config);

              motorcommand[0] = SONAR;
              motorcommand[1] = 0x3B;
              motorcommand[2] = sonar_config.value;
              motorcommand[3] = 0;
              motorpacket.Build(motorcommand, 4);
              SendReceive(&motorpacket);

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;

            case PLAYER_SONAR_GET_GEOM_REQ:
              /* Return the sonar geometry. */
              if(config_size != 1)
              {
                puts("Arg get sonar geom is wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }

              player_sonar_geom_t geom;
              geom.subtype = PLAYER_SONAR_GET_GEOM_REQ;
              geom.pose_count = htons(PlayerRobotParams[param_idx].SonarNum);
              for (int i = 0; i < PLAYER_SONAR_MAX_SAMPLES; i++)
              {
                sonar_pose_t pose = PlayerRobotParams[param_idx].sonar_pose[i];
                geom.poses[i][0] = htons((short) (pose.x));
                geom.poses[i][1] = htons((short) (pose.y));
                geom.poses[i][2] = htons((short) (pose.th));
              }

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, 
                          sizeof(geom)))
                PLAYER_ERROR("failed to PutReply");
              break;
            default:
              puts("Sonar got unknown config request");
              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK,
                          NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;
          }
          break;
        case PLAYER_POSITION_CODE:
          switch(config[0])
          {
            case PLAYER_POSITION_SET_ODOM_REQ:
              if(config_size != sizeof(player_position_set_odom_req_t))
              {
                puts("Arg to odometry set requests wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }

              player_position_set_odom_req_t set_odom_req;
              set_odom_req = *((player_position_set_odom_req_t*)config);