entity.cc

一个机器人平台

    case PropSonarReturn:
      memcpy( &sonar_return, (bool*)value, sizeof(sonar_return) );
      break;
    case PropObstacleReturn:
      memcpy( &obstacle_return, (bool*)value, sizeof(obstacle_return) );
      break;
    case PropVisionReturn:
      memcpy( &vision_return, (bool*)value, sizeof(vision_return));
      break;
    case PropPuckReturn:
      memcpy( &puck_return, (bool*)value, sizeof(puck_return) );
      break;

    default:
      //printf( "Stage Warning: attempting to set unknown property %d\n", 
      //      property );
      break;
  }
  
  // indicate that the property is dirty on all _but_ the connection
  // it came from - that way it gets propogated onto to other clients
  // and everyone stays in sync. (assuming no recursive connections...)
  
  this->SetDirty( property, 1 ); // dirty on all cons
  
  if( con != -1 ) // unless this was a local change 
    this->SetDirty( con, property, 0 ); // clean on this con

  if( refresh_figure )
    {
#ifdef INCLUDE_RTK2 
      if( this->fig )
	{
	  RtkShutdown();
	  RtkStartup();
	}
#endif
    }
  
  if( move_figure )
    {
      //printf( "-- moving figure to %.2f %.2f %.2f\n",
      //      local_px, local_py, local_pth );

#ifdef INCLUDE_RTK2    
      if( this->fig )
	rtk_fig_origin(this->fig, local_px, local_py, local_pth );
#endif 
      
      //#ifdef USE_GNOME2 
      //this->GuiMove();
      //#endif
    }

  // update the GUI with the new property
  if( m_world->enable_gui )
    GuiEntityPropertyChange( this, property );
  
  return 0;
}


int CEntity::GetProperty( EntityProperty property, void* value )
{
  //PRINT_DEBUG1( "finding property %d", property );
  //printf( "finding property %d", property );

  assert( value );

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

  switch( property )
  {
    case PropParent:
      // TODO - fix
      // find the parent's position in the world's entity array
      // if parent pointer is null or otherwise invalid, index is -1 
      //{ int parent_index = m_world->GetEntityIndex( m_parent_entity );

      { 
	int parent_index = -1;

	if( m_parent_entity )
	  parent_index = m_parent_entity->stage_id ;
	
	memcpy( value, &parent_index, sizeof(parent_index) );
	retval = sizeof(parent_index); 
      }
    break;
    case PropSizeX:
      memcpy( value, &size_x, sizeof(size_x) );
      retval = sizeof(size_x);
      break;
    case PropSizeY:
      memcpy( value, &size_y, sizeof(size_y) );
      retval = sizeof(size_y);
      break;
    case PropPoseX:
      memcpy( value, &local_px, sizeof(local_px) );
      retval = sizeof(local_px);
      break;
    case PropPoseY:
      memcpy( value, &local_py, sizeof(local_py) );
      retval = sizeof(local_py);
      break;
    case PropPoseTh:
      memcpy( value, &local_pth, sizeof(local_pth) );
      retval = sizeof(local_pth);
      break;
    case PropOriginX:
      memcpy( value, &origin_x, sizeof(origin_x) );
      retval = sizeof(origin_x);
      break;
      break;
    case PropOriginY:
      memcpy( value, &origin_y, sizeof(origin_y) );
      retval = sizeof(origin_y);
      break;
    case PropName:
      strcpy( (char*)value, name );
      retval = strlen(name);
      break;
    case PropColor:
      memcpy( value, &color, sizeof(color) );
      retval = sizeof(color);
      break;
    case PropShape:
      memcpy( value, &shape, sizeof(shape) );
      retval = sizeof(shape);
      break;
    case PropLaserReturn:
      memcpy( value, &laser_return, sizeof(laser_return) );
      retval = sizeof(laser_return);
      break;
    case PropSonarReturn:
      memcpy( value, &sonar_return, sizeof(sonar_return) );
      retval = sizeof(sonar_return);
      break;
    case PropIdarReturn:
      memcpy( value, &idar_return, sizeof(idar_return) );
      retval = sizeof(idar_return);
      break;
    case PropObstacleReturn:
      memcpy( value, &obstacle_return, sizeof(obstacle_return) );
      retval = sizeof(obstacle_return);
      break;
    case PropVisionReturn:
      memcpy( value, &vision_return, sizeof(vision_return) );
      retval = sizeof(vision_return);
      break;
    case PropPuckReturn:
      memcpy( value, &puck_return, sizeof(puck_return) );
      retval = sizeof(puck_return);
      break;
    default:
      // printf( "Stage Warning: attempting to get unknown property %d\n", 
      //      property );
    break;
  }

  return retval;
}

// write the entity tree onto the console
void CEntity::Print( char* prefix )
{
  double ox, oy, oth;
  this->GetGlobalPose( ox, oy, oth );

  printf( "%s type: %s global: [%.2f,%.2f,%.2f]"
	  " local: [%.2f,%.2f,%.2f] vision_return %d )", 
	  prefix,
	  this->lib_entry->token,
	  ox, oy, oth,
	  local_px, local_py, local_pth,
	  this->vision_return );
	  
  if( this->m_parent_entity == NULL )
    puts( " - ROOT" );
  else
    puts( "" );

  // add an indent to the prefix
  
  char* buf = new char[ strlen(prefix) + 1 ];
  sprintf( buf, "\t%s", prefix );

  CHILDLOOP( ch )
    ch->Print( buf );
}


// subscribe to / unsubscribe from the device
// these don't do anything by default, but are overridden by CPlayerEntity
void CEntity::Subscribe()
{ 
  //puts( "SUB" );
};

void CEntity::Unsubscribe()
{ 
  //puts( "UNSUB" );
};
  
int CEntity::Subscribed()
{ 
  return 0;
};


// these versions sub/unsub to this device and all its decendants
void CEntity::FamilySubscribe()
{ 
  CHILDLOOP( ch ) ch->FamilySubscribe(); 
};

void CEntity::FamilyUnsubscribe()
{ 
  CHILDLOOP( ch ) ch->FamilyUnsubscribe(); 
};


void CEntity::GuiStartup( void )
{
  // use the interface library hook
  GuiEntityStartup( this );
  
  CHILDLOOP( ch )
    ch->GuiStartup();
}


void CEntity::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) Stage(%d:%d(%s))",
		    x, y, th, 
		    this->stage_id,
		    this->lib_entry->type_num,
		    this->lib_entry->token ) );
}  

#ifdef INCLUDE_RTK2

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

  PRINT_DEBUG2("RTK STARTUP %s %s",
               this->lib_entry->token,
               m_parent_entity ? "" : " - ROOT" );

  // Create a figure representing this entity
  if( m_parent_entity == NULL )
    this->fig = rtk_fig_create(m_world->canvas, NULL, 50);
  else
    this->fig = rtk_fig_create(m_world->canvas, m_parent_entity->fig, 50);

  assert( this->fig );

  /* REMOVE
  this->fig->thing = (void*)this;
  this->fig->origin_callback = staticSetGlobalPose;
  this->fig->select_callback = NULL;
  this->fig->unselect_callback = NULL;
  */

  // Set the mouse handler
  this->fig->userdata = this;
  rtk_fig_add_mouse_handler(this->fig, StaticRtkOnMouse);

  // add this device to the world's device menu 
  this->m_world->AddToDeviceMenu( this, true); 
    
  // visible by default
  rtk_fig_show( this->fig, true );

  // Set the color
  rtk_fig_color_rgb32(this->fig, this->color);

  // put the figure's origin at the entity's position
  rtk_fig_origin( this->fig, local_px, local_py, local_pth );


#ifdef RENDER_INITIAL_BOUNDING_BOXES
  double xmin, ymin, xmax, ymax;
  xmin = ymin = 999999.9;
  xmax = ymax = 0.0;
  this->GetBoundingBox( xmin, ymin, xmax, ymax );
  
  rtk_fig_t* boundaries = rtk_fig_create( m_world->canvas, NULL, 99);
  double width = xmax - xmin;
  double height = ymax - ymin;
  double xcenter = xmin + width/2.0;
  double ycenter = ymin + height/2.0;

  rtk_fig_rectangle( boundaries, xcenter, ycenter, 0, width, height, 0 ); 

   
#endif
   
  // draw the shape using the center of rotation offsets
  switch (this->shape)
  {
    case ShapeRect:
      rtk_fig_rectangle(this->fig, 
                        this->origin_x, this->origin_y, 0, 
                        this->size_x, this->size_y, false);
      break;
    case ShapeCircle:
      rtk_fig_ellipse(this->fig, 
                      this->origin_x, this->origin_y, 0,  
                      this->size_x, this->size_y, false);
      break;
    case ShapeNone: // no shape
      break;
  }
  

  // everything except the root object has a label
  if( m_parent_entity )
  {
    // Create the label
    // By default, the label is not shown
    this->fig_label = rtk_fig_create(m_world->canvas, this->fig, 51);
    rtk_fig_show(this->fig_label, false);    
    rtk_fig_movemask(this->fig_label, 0);
      
    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));
      
    rtk_fig_color_rgb32(this->fig, this->color);
    rtk_fig_text(this->fig_label,  0.75 * size_x,  0.75 * size_y, 0, label);
      
    // attach the label to the main figure
    // rtk will draw the label when the mouse goes over the figure
    // TODO: FIX
    //this->fig->mouseover_fig = fig_label;
      
    // we can be moved only if we are on the root node
    if (m_parent_entity != this->m_world->GetRoot() )
      rtk_fig_movemask(this->fig, 0);
    else
      rtk_fig_movemask(this->fig, this->movemask);  
  }

  // do our children after we're set
  CHILDLOOP( child ) child->RtkStartup();
  PRINT_DEBUG( "RTK STARTUP DONE" );
}



///////////////////////////////////////////////////////////////////////////
// Finalise the rtk gui
void CEntity::RtkShutdown()
{
  // Clean up the figure we created
  rtk_fig_destroy(this->fig);
  rtk_fig_destroy(this->fig_label);
} 


///////////////////////////////////////////////////////////////////////////
// Update the rtk gui
void CEntity::RtkUpdate()
{
  CHILDLOOP( child ) child->RtkUpdate();

  // TODO this is nasty and inefficient - figure out a better way to
  // do this  

  // if we're not looking at this device, hide it 
  if( !m_world->ShowDeviceBody( this->lib_entry->type_num ) )
  {
    rtk_fig_show(this->fig, false);
  }
  else // we need to show and update this figure
  {
    rtk_fig_show( this->fig, true );
  }
}


///////////////////////////////////////////////////////////////////////////
// Process mouse events
void CEntity::RtkOnMouse(rtk_fig_t *fig, int event, int mode)
{
  double px, py, pth;

  switch (event)
  {
    case RTK_EVENT_PRESS:
    case RTK_EVENT_MOTION:
    case RTK_EVENT_RELEASE:
      rtk_fig_get_origin(fig, &px, &py, &pth);
      this->SetGlobalPose(px, py, pth);
      break;

    default:
      break;
  }

  return;
}


///////////////////////////////////////////////////////////////////////////
// Process mouse events (static callback)
void CEntity::StaticRtkOnMouse(rtk_fig_t *fig, int event, int mode)
{
  CEntity *entity;
  entity = (CEntity*) fig->userdata;
  entity->RtkOnMouse(fig, event, mode);
  return;
}

#endif