/* *  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}///////////////////////////////////////////////////////////////////////////// DestructorCEntity::~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 sectionCEntity* 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 filebool 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 filebool 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;