playerdevice.cc

一个机器人平台

  // just to be sure
  this->m_info_io->drivername[sizeof(this->m_info_io->drivername)-1] = '\0';

  Unlock();
}

////////////////////////////////////////////////////////////////////////////
// Copy the data into shared memory & update the info buffer
size_t CPlayerEntity::PutData( void* data, size_t len )
{
  // tell the server to export this data to anyone that needs it
  this->SetDirty( PropData, 1 );
  
  //PRINT_DEBUG4( "Putting %d bytes of data into device (%d,%d,%d)\n",
  //	len, m_player.port, m_player.type, m_player.index );
  
  // copy the data into the mmapped data buffer.
  // also set the timestamp and available fields for the data
  size_t retval = PutIOData( (void*)m_data_io, m_data_len,
			     data, len,
			     &m_info_io->data_timestamp_sec,
			     &m_info_io->data_timestamp_usec,
			     &m_info_io->data_avail );
  
  // give the GUI a chance to display this data
  GuiEntityPropertyChange( this, PropData );

  return retval;
}

////////////////////////////////////////////////////////////////////////////
// Copy the command into shared memory & update the info buffer
size_t CPlayerEntity::PutCommand( void* data, size_t len )
{
  // tell the server to export this command to anyone that needs it
  this->SetDirty( PropCommand, 1 );
  
  // copy the data into the mmapped command buffer.
  // also set the timestamp and available fields for the command
  return PutIOData( m_command_io, m_command_len,
		    data, len,
		    &m_info_io->command_timestamp_sec,
		    &m_info_io->command_timestamp_usec,
		    &m_info_io->command_avail );
  
  
}


///////////////////////////////////////////////////////////////////////////
// Read a configuration from the shared memory
size_t CPlayerEntity::GetConfig(void** client, void* config, size_t len )
{
  int size;
  player_device_id_t id;

  Lock();

  if((size = m_reqqueue->Pop(&id, client, (unsigned char*)config, len)) < 0)
  {
    Unlock();
    return(0);
  }

  // tell the server to export this config to anyone that needs it
  this->SetDirty( PropConfig, 1 );

  Unlock();
  return(size);
}

size_t CPlayerEntity::PutReply(void* client, unsigned short type,
                         struct timeval* ts, void* reply, size_t len)
{
  double seconds;
  struct timeval curr;
  
  // tell the server to export this reply to anyone that needs it
  this->SetDirty( PropReply, 1 );

  if(ts)
    curr = *ts;
  else
  {
    seconds = m_world->GetTime();
    curr.tv_sec = (long)seconds;
    curr.tv_usec = (long)((seconds - curr.tv_sec)*1000000);
  }

  Lock();
  m_repqueue->Push(NULL, client, type, &curr, (unsigned char*)reply, len);
  Unlock();
  return(0);
}


///////////////////////////////////////////////////////////////////////////
// Put a reply back on the reply queue.
size_t CPlayerEntity::PutReply(void* client, unsigned short type)
{
  return(PutReply(client, type, NULL, NULL, 0));
} 


///////////////////////////////////////////////////////////////////////////
// See if the PlayerDevice is subscribed
int CPlayerEntity::Subscribed() 
{
  int subscribed = 0; 
  
  if( m_info_io ) // if we have player connection at all
    { 
      Lock();
      // see if a player client is connected to this entity
      subscribed = m_info_io->subscribed;
      Unlock();
    }
  
  //printf( "SUBS: %d\n", subscribed );


  return( subscribed );
}

void CPlayerEntity::FamilySubscribe()
{
  CHILDLOOP( ch )
    ch->FamilySubscribe();

  this->Subscribe();
}

void CPlayerEntity::FamilyUnsubscribe()
{
  CHILDLOOP( ch )
    ch->FamilyUnsubscribe();

  this->Unsubscribe();
}

///////////////////////////////////////////////////////////////////////////
//// subscribe to the device (used by other devices that depend on this one)
void CPlayerEntity::Subscribe() 
{
  Lock();
  m_info_io->subscribed++;  
  Unlock(); 
  
  //printf( "player %d.%d.%d subs %d\n", 
  //  this->m_player.port,  
  //  this->m_player.code,  
  //  this->m_player.index,
  //  m_info_io->subscribed );

  CEntity::Subscribe();
}

///////////////////////////////////////////////////////////////////////////
// unsubscribe from the device (used by other devices that depend on this one)
void CPlayerEntity::Unsubscribe()
{ 
  //puts( "PUNSUB" );
  
  Lock();
  if( m_info_io->subscribed > 0 )
    m_info_io->subscribed--;
  Unlock();

  CEntity::Unsubscribe();
} 


///////////////////////////////////////////////////////////////////////////
// lock the shared mem
// Returns true on success
//
bool CPlayerEntity::Lock( void )
{
  assert(m_world);
  return m_world->LockByte( this->lock_byte );
}

///////////////////////////////////////////////////////////////////////////
// unlock the shared mem
//
bool CPlayerEntity::Unlock( void )
{
  assert(m_world);
  return m_world->UnlockByte( this->lock_byte );
}

int CPlayerEntity::SetProperty( int con, EntityProperty property, 
			  void* value, size_t len )
{
  PRINT_DEBUG( "" );

  assert( value );
  assert( len > 0 );
  assert( (int)len < MAX_PROPERTY_DATA_LEN );

  switch( property )
  {
    case PropPlayerSubscriptions:
      PRINT_DEBUG1( "PLAYER SUBSCRIPTIONS %d", *(int*) value);
      
      if( m_info_io )
      {
        Lock();
        m_info_io->subscribed = *(int*)value;
        Unlock();
      }      
      break;
      
    case PropPlayerId:
      memcpy( &m_player, (player_device_id_t*)value, sizeof(m_player) );
      break;
      
      // these properties manipulate the player IO buffers
    case PropCommand:
      PutCommand( value, len );
      break;
      
    case PropData:
      PutData( value, len );
      break;
      
    case PropConfig: // copy the  playerqueue's external memory chunk
    { 
      Lock();
      size_t len = m_config_len * sizeof(playerqueue_elt_t);
      memcpy( m_config_io, value, len ); 
      Unlock();
    }
    break;
  
    case PropReply:
    { 
      Lock();
      size_t len = m_reply_len * sizeof(playerqueue_elt_t);
      memcpy( m_reply_io, value, len ); 
      Unlock();
    }
    break;
      
    default: // assume it'll be handled by CEntity::SetProperty
      break; 
  }

  // even if we handled it, we still call the basic SetProperty in
  // order to set the dirty flags correctly
  return( CEntity::SetProperty( con, property, value, len ) ); 
}


int CPlayerEntity::GetProperty( EntityProperty property, void* value )
{ 
  PRINT_DEBUG3( "finding property %d for entity (%d %s)", 
                property, this->stage_id, this->lib_entry->token );

  assert( value );

  // indicate no data - this should be overridden below
  int retval = 0;

  switch( property )
  {
    case PropPlayerSubscriptions:
      PRINT_DEBUG( "GET SUBS PROPERTY");
      { int subs = Subscribed();
      memcpy( value, (void*)&subs, sizeof(subs) ); 
      retval = sizeof(subs); }
      break;

    case PropPlayerId:
      memcpy( value, &m_player, sizeof(m_player) );
      retval = sizeof(m_player);
      break;

      // these properties manipulate the player IO buffers
    case PropCommand:
      retval = GetCommand( value, m_command_len );
      break;

    case PropData:
      retval = GetData( value, m_data_len );
      break;

    case PropConfig:
    { 
      Lock();
      size_t len = m_config_len * sizeof(playerqueue_elt_t);
      memcpy( value, m_config_io, len );
      retval = len; 
      Unlock();
    }
    break;
    
    case PropReply:
    { 
      Lock();
      size_t len = m_reply_len * sizeof(playerqueue_elt_t);
      memcpy( value, m_reply_io, len );
      retval = len; 
      Unlock();
    }
    break;

    default:
      // we didn't have that proprty code - pass request down to the CEntity and return
      return( CEntity::GetProperty( property, value ) );
  }

  return retval;
}

void CPlayerEntity::GetStatusString( char* buf, int buflen )
{
  double x, y, th;
  this->GetGlobalPose( x, y, th );
  
  // check for overflow
  assert( -1 !=
	  snprintf( buf, buflen, 
		    "Pose(%.2f,%.2f,%.2f) Player(%d:%d:%d) Stage(%d:%d(%s))",
		    x, 
		    y, 
		    th,
		    this->m_player.port, 
		    this->m_player.code, 
		    this->m_player.index, 
		    this->stage_id,
		    this->lib_entry->type_num,
		    this->lib_entry->token
		    ) );
 
}

#ifdef INCLUDE_RTK2

// Initialise the rtk gui
void CPlayerEntity::RtkStartup()
{
  CEntity::RtkStartup();

  // add this device to the world's data menu 
  this->m_world->AddToDataMenu( this, true); 

  // add the player ID string to the label figure
  char label[1024];
  char tmp[1024];
   
  label[0] = 0;
  snprintf(tmp, sizeof(tmp), "%s %s", 
	   this->name,
	   this->lib_entry->token );
  strncat(label, tmp, sizeof(label));
  if (m_player.port > 0)
  {
    snprintf(tmp, sizeof(tmp), "\n%d:%d", m_player.port, m_player.index);
    strncat(label, tmp, sizeof(label));
  }
   
  rtk_fig_clear(this->fig_label );
  rtk_fig_text(this->fig_label,  0.75 * size_x,  0.75 * size_y, 0, label);

  return;
}


///////////////////////////////////////////////////////////////////////////
// Process mouse events
void CPlayerEntity::RtkOnMouse(rtk_fig_t *fig, int event, int mode)
{
  switch (event)
  {
    case RTK_EVENT_PRESS:
    
      // if the CTRL key is held down, launch playerv
      // rtk doesn't support modifiers directly so we use this
      // backdoor route which may not sync perfectly with the click
      // itself over a delayed network, but it should work pretty well.
      int x, y;
      GdkModifierType modifiers;
      gdk_window_get_pointer(m_world->canvas->canvas->window, 
                             &x, &y, &modifiers);
    
      //if( modifiers & GDK_CONTROL_MASK ) SpawnPlayerv();
      break;

      // Handle case when mouse is moved over the figure
    case RTK_EVENT_MOUSE_OVER:
      this->FamilySubscribe();
      break;
      
      // Handle case when mouse is moved away from the figure
    case RTK_EVENT_MOUSE_NOT_OVER:
      this->FamilyUnsubscribe();
      break;
      
    default:
      break;
  }
  
  CEntity::RtkOnMouse(fig, event, mode);
  return;
}

#endif