entity.cc

一个机器人平台

/*
 *  Stage : a multi-robot simulator.
 *  Copyright (C) 2001, 2002 Richard Vaughan, Andrew Howard and Brian Gerkey.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 * Desc: Base class for every entity.
 * Author: Richard Vaughan, Andrew Howard
 * Date: 7 Dec 2000
 * CVS info: $Id: entity.cc,v 1.96.2.2 2003/05/24 01:11:19 inspectorg Exp $
 */
#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#include <float.h> // for FLT_MAX
#include <sys/mman.h>
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/file.h>

#include <iostream>


//#define DEBUG
//#define VERBOSE
//#undef DEBUG
//#undef VERBOSE
//#define RENDER_INITIAL_BOUNDING_BOXES

#include "entity.hh"
#include "raytrace.hh"
#include "world.hh"
#include "worldfile.hh"
#include "gui.hh"
#include "library.hh"


///////////////////////////////////////////////////////////////////////////
// main constructor
// Requires a pointer to the parent and a pointer to the world.
CEntity::CEntity(LibraryItem* libit, CWorld *world, CEntity *parent_entity )
{
  assert( world );
  
  this->m_world = world; 
  this->m_parent_entity = parent_entity;
  this->m_default_entity = this;
  
  // store the library's entry for this type
  this->lib_entry = libit;

  // get the default color
  this->color = lib_entry->color;

  // attach to my parent
  if( m_parent_entity )
    m_parent_entity->AddChild( this );
  else
    PRINT_DEBUG1( "ROOT ENTITY %p\n", this );
  
  // register with the world to receive our unique id
  this->stage_id = m_world->RegisterEntity( this );

  // init the child list data
  this->child_list = this->prev = this->next = NULL;

  // Set our default name
  this->name[0] = 0;

  // Set default pose
  this->local_px = this->local_py = this->local_pth = 0;

  // Set global velocity to stopped
  this->vx = this->vy = this->vth = 0;

  // unmoveably MASSIVE! by default
  this->mass = 1000.0;
    
  // Set the default shape and geometry
  this->shape = ShapeNone;
  this->size_x = this->size_y = 0;
  this->origin_x = this->origin_y = 0;

  // set the default worldfile section to be 0 (top-level)
  this->worldfile_section = 0;

  m_local = false; 
  
  // by default, entities don't show up in any sensors
  // these must be enabled explicitly in each subclass
  obstacle_return = false;
  sonar_return = false;
  puck_return = false;
  vision_return = false;
  laser_return = LaserTransparent;
  idar_return = IDARTransparent;
  fiducial_return = FiducialNone; // not a recognized fiducial
  gripper_return = GripperDisabled;

  // Set the initial mapped pose to a dummy value
  this->map_px = this->map_py = this->map_pth = 0;

  m_dependent_attached = false;

  // we start out NOT dirty - no one gets deltas unless they ask for 'em.
  SetDirty( 0 );
  
  m_last_pixel_x = m_last_pixel_y = m_last_degree = 0;

  m_interval = 0.1; // update interval in seconds 
  m_last_update = -FLT_MAX; // initialized 

  // init the ptr to GUI-specific data
  this->gui_data = NULL;

#ifdef INCLUDE_RTK2
  // Default figures for drawing the entity.
  this->fig = NULL;
  this->fig_label = NULL;

  // By default, we can both translate and rotate the entity.
  this->movemask = RTK_MOVE_TRANS | RTK_MOVE_ROT;
#endif

}


///////////////////////////////////////////////////////////////////////////
// Destructor
CEntity::~CEntity()
{
}

void CEntity::AddChild( CEntity* child )
{
  //printf( "appending entity %p to parent %p\n", child, this );
  
  STAGE_LIST_APPEND( this->child_list, child ); 
}

// returns true if this object is a child of ancestor, or a child
// of a child of ancestor, etc, recursively.
/*
bool CEntity::IsDescendent( CEntity* ancestor )
{
  if( m_parent_entity == NULL )
    return false;
  
  if( m_parent_entity == ancestor )
    return true;
  
  // recurse
  return( m_parent_entity->IsDescendent( ancestor ) );
}
*/


void CEntity::GetBoundingBox( double &xmin, double &ymin,
			      double &xmax, double &ymax )
{
  double x[4];
  double y[4];

  double dx = size_x / 2.0;
  double dy = size_y / 2.0;
  double dummy = 0.0;
  
  x[0] = origin_x + dx;
  y[0] = origin_y + dy;
  this->LocalToGlobal( x[0], y[0], dummy );

  x[1] = origin_x + dx;
  y[1] = origin_y - dy;
  this->LocalToGlobal( x[1], y[1], dummy );

  x[2] = origin_x - dx;
  y[2] = origin_y + dy;
  this->LocalToGlobal( x[2], y[2], dummy );

  x[3] = origin_x - dx;
  y[3] = origin_y - dy;
  this->LocalToGlobal( x[3], y[3], dummy );

  //double ox, oy, oth;
  //GetGlobalPose( ox, oy, oth );
  //printf( "origin: %.2f,%.2f,%.2f \n", ox, oy, oth );
  //printf( "corners: \n" );
  //for( int c=0; c<4; c++ )
  //printf( "\t%.2f,%.2f\n", x[c], y[c] );
  
  // find the smallest values and we're done
  //xmin = xmax = x[0];
  for( int c=0; c<4; c++ )
    {
      if( x[c] < xmin ) xmin = x[c];
      if( x[c] > xmax ) xmax = x[c];
    }

  //ymin = ymax = y[0];
  for( int c=0; c<4; c++ )
    {
      if( y[c] < ymin ) ymin = y[c];
      if( y[c] > ymax ) ymax = y[c];
    } 

  //printf( "before children: %.2f %.2f %.2f %.2f\n",
  //  xmin, ymin, xmax, ymax );

  CHILDLOOP( ch )
    ch->GetBoundingBox(xmin, ymin, xmax, ymax ); 

  //printf( "after children: %.2f %.2f %.2f %.2f\n",
  //  xmin, ymin, xmax, ymax );

}

// this is called very rapidly from the main loop
// it allows the entity to perform some actions between clock increments
// (such handling config requests to increase synchronous IO performance)
void CEntity::Sync()
{ 
  // default - do nothing but call the children
  CHILDLOOP( ch ) ch->Sync(); 
};


// return a pointer to this or a child if it matches the worldfile section
CEntity* CEntity::FindSectionEntity( int section )
{
  //PRINT_DEBUG2( "find section %d. my section %d\n", 
  //	section, this->worldfile_section );

  CEntity* found = NULL;
  
  if( section == this->worldfile_section )
    found  = this;
  
  //int f=0;
  // otherwise, recursively check our children
  if( found == NULL ) CHILDLOOP( ch )
    {
      //printf( "looking in child %d\n", f++ ); 
      found = ch->FindSectionEntity( section ); 
      if( found ) break;
    }
  
  //printf( "found %s\n", found ? "true" : "false" );
  
  return found;
  
}



///////////////////////////////////////////////////////////////////////////
// Load the entity from the world file
bool CEntity::Load(CWorldFile *worldfile, int section)
{
  // Read the name
  strcpy( this->name, worldfile->ReadString(section, "name", "") );
      
  // Read the pose
  this->init_px = worldfile->ReadTupleLength(section, "pose", 0, 0);
  this->init_py = worldfile->ReadTupleLength(section, "pose", 1, 0);
  this->init_pth = worldfile->ReadTupleAngle(section, "pose", 2, 0);
  SetPose(this->init_px, this->init_py, this->init_pth);

  // Read the shape
  const char *shape_desc = worldfile->ReadString(section, "shape", NULL);
  if (shape_desc)
  {
    if (strcmp(shape_desc, "rect") == 0)
      this->shape = ShapeRect;
    else if (strcmp(shape_desc, "circle") == 0)
      this->shape = ShapeCircle;
    else
      PRINT_WARN1("invalid shape desc [%s]; using default", shape_desc);
  }

  // Read the size
  this->size_x = worldfile->ReadTupleLength(section, "size", 0, this->size_x);
  this->size_y = worldfile->ReadTupleLength(section, "size", 1, this->size_y);

  // Read the origin offsets (for moving center of rotation)
  this->origin_x = worldfile->ReadTupleLength(section, "offset", 0, this->origin_x);
  this->origin_y = worldfile->ReadTupleLength(section, "offset", 1, this->origin_y);

  // Read the entity color
  this->color = worldfile->ReadColor(section, "color", this->color);

  // read the desired update interval
  this->m_interval = 
    worldfile->ReadFloat( section, "interval", this->m_interval );
 
  const char *rvalue;
  
  // Obstacle return values
  if (this->obstacle_return)
    rvalue = "visible";
  else
    rvalue = "invisible";
  rvalue = worldfile->ReadString(section, "obstacle_return", rvalue);
  if (strcmp(rvalue, "visible") == 0)
    this->obstacle_return = true;
  else
    this->obstacle_return = false;

  // Sonar return values
  if (this->sonar_return)
    rvalue = "visible";
  else
    rvalue = "invisible";
  rvalue = worldfile->ReadString(section, "sonar_return", rvalue);
  if (strcmp(rvalue, "visible") == 0)
    this->sonar_return = true;
  else
    this->sonar_return = false;

  // Vision return values
  if (this->vision_return)
    rvalue = "visible";
  else
    rvalue = "invisible";
  rvalue = worldfile->ReadString(section, "vision_return", rvalue);
  if (strcmp(rvalue, "visible") == 0)
    this->vision_return = true;
  else
    this->vision_return = false;

  // Gripper return values
  if (this->gripper_return)
    rvalue = "visible";
  else
    rvalue = "invisible";
  rvalue = worldfile->ReadString(section, "gripper_return", rvalue);
  if (strcmp(rvalue, "visible") == 0)
    this->gripper_return = GripperEnabled;
  else
    this->gripper_return = GripperDisabled;

  // Read the beacon id
  this->fiducial_return \
    = worldfile->ReadInt(section, "fiducial_id", this->fiducial_return );

  // Use the beacon id as a name if there is no name set
  if ( strlen(this->name) == 0 && this->fiducial_return != FiducialNone )
    snprintf( this->name, 64, "id %d", this->fiducial_return );
  
  // Laser return values
  if (this->laser_return == LaserBright)
    rvalue = "bright";
  else if (this->laser_return == LaserVisible)
    rvalue = "visible";
  else
    rvalue = "invisible";
  rvalue = worldfile->ReadString(section, "laser_return", rvalue);
  if (strcmp(rvalue, "bright") == 0)
    this->laser_return = LaserBright;
  else if (strcmp(rvalue, "visible") == 0)
    this->laser_return = LaserVisible;
  else
    this->laser_return = LaserTransparent;
    
  return true;
}


///////////////////////////////////////////////////////////////////////////
// Save the entity to the world file
bool CEntity::Save(CWorldFile *worldfile, int section)
{
  // Write the pose (but only if it has changed)
  double px, py, pth;
  GetPose(px, py, pth);

  //printf( "pose: %.2f %.2f %.2f   init:  %.2f %.2f %.2f\n",
  //  px, py, pth, init_px, init_py, init_pth );

  // TODO - save out any other changed properties

  if (px != this->init_px || py != this->init_py || pth != this->init_pth)
  {
    worldfile->WriteTupleLength(section, "pose", 0, px);
    worldfile->WriteTupleLength(section, "pose", 1, py);
    worldfile->WriteTupleAngle(section, "pose", 2, pth);
  }

  CHILDLOOP( ch ) ch->Save( worldfile, ch->worldfile_section );
  
  return true;
}


///////////////////////////////////////////////////////////////////////////
// Startup routine
// A virtual function that lets entities do some initialization after
// everything has been loaded.
bool CEntity::Startup( void )
{
  PRINT_DEBUG2("ENTITY STARTUP %s %s", 
         this->lib_entry->token,
         m_parent_entity ? "" : "- ROOT" );
  
  // use the generic hook
  if( m_world->enable_gui )
    GuiEntityStartup( this );
  

 CHILDLOOP( ch )
    ch->Startup();

  return true;