idardevice.cc

一个机器人平台

/******************************************************************************
* File: irdevice.cc
* Author: Richard Vaughan
* Date: 22 October 2001
* Desc: Simulates a single sensor of HRL's Infrared Data and Ranging Device
*
* 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
*
* CVS info:
* $Source: /cvsroot/playerstage/code/stage/src/models/Attic/idardevice.cc,v $
* $Author: rtv $
* $Revision: 1.2 $
******************************************************************************/


#include <math.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>

#include <iostream>

#include "world.hh"
#include "idardevice.hh"
#include "raytrace.hh"

#define DEBUG
//#undef DEBUG

// control display of player index in device body
//#define RENDER_INDEX
//#define RENDER_SCANLINES
//#define SHOW_MSGS

#define IDAR_TRANSMIT_ANGLE        M_PI/4.0
#define IDAR_RECEIVE_ANGLE        M_PI/4.0
#define IDAR_SCANLINES        5
#define IDAR_MAX_RANGE       1.0

// handy circular normalization macros
#define NORMALIZECIRCULAR_INT(value,max) (((value%max)+max)%max)
#define NORMALIZECIRCULAR_DOUBLE(value,max) fmod( fmod(value,max)+max, max)

#define NORMALIZERADIANS(value)  NORMALIZECIRCULAR_DOUBLE(value,TWOPI)
#define NORMALIZEDEGREES(value)  NORMALIZECIRCULAR_DOUBLE(value,360.0)


// constructor 
CIdarDevice::CIdarDevice(LibraryItem* libit, CWorld *world, CEntity *parent )
  : CPlayerEntity(libit, world, parent )
{
  // we're invisible except to other IDARs
  laser_return = LaserTransparent;
  sonar_return = false;
  obstacle_return = false;
  idar_return = IDARReceive;

  // set the Player IO sizes correctly for this type of Entity
  m_data_len    = 0;//sizeof( player_idar_data_t );
  m_command_len = 0;//sizeof( player_idar_command_t );
  m_config_len  = 1;
  m_reply_len  = 1;
  

  m_player.code = PLAYER_IDAR_CODE; // from player's messages.h
    
  m_max_range = IDAR_MAX_RANGE;
    
  size_x = 0.03; // this is the actual physical size of the HRL device
  size_y = 0.02; // but can be changed in the world file to suit

  // Set the default shape
  shape = ShapeRect;

  m_interval = 0.001; // updates frequently,
  // but only has work to do when it receives a command
  // so it's not a CPU hog unless we send it lots of messages
 
  // init the message buffer
  memset( &recv, 0, sizeof(recv) );

  m_num_scanlines = IDAR_SCANLINES; 
  m_angle_per_sensor = IDAR_TRANSMIT_ANGLE;
  m_angle_per_scanline = m_angle_per_sensor / m_num_scanlines;
}

///////////////////////////////////////////////////////////////////////////
// Startup routine
//
bool CIdarDevice::Startup()
{
  if (!CPlayerEntity::Startup())
    return false;

  return true;
}

void CIdarDevice::Sync( void )
{
  // sync our children
  CPlayerEntity::Sync();

  void *client;
  player_idar_config_t cfg;
  
  // Get config
  int res = GetConfig( &client, &cfg, sizeof(cfg));
  
  switch( res )
    { 
    case 0:
      // nothing available - nothing to do
      break;
      
    case -1: // error
      PRINT_ERR( "get config failed" );
      break;
      
    case sizeof(cfg): // the size we expect - we received a config request
      // what does the client want us to do?
      switch( cfg.instruction )
	{
	case IDAR_TRANSMIT:
	  //puts( "TX" );
	  TransmitMessage( &(cfg.tx) );
	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK );
	  
	  break;
	  
	case IDAR_RECEIVE:
	  //puts( "RX" );
	  // send back the currently stored message
	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));
	  ClearMessage(); // wipe the buffer
	  break;
	  
	case IDAR_RECEIVE_NOFLUSH:
	  //puts( "RX_NF" );
	  // send back the currently stored message
	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));
	  // don't clear the current mesg
	  break;
	  
	  // two in one for efficiency!
	case IDAR_TRANSMIT_RECEIVE:
	  TransmitMessage( &(cfg.tx) );
	  PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &recv, sizeof(recv));
	  ClearMessage(); // wipe the buffer
	  break;

	default:
	  printf( "STAGE warning: unknown idar config instruction %d\n",
		  cfg.instruction );
	}
      break;
      
    default: // a bad value
      PRINT_ERR1( "wierd: idar config returned %d ", res );
      break;
    }
}


// NOTE unlike most devices, live data is not published here - that's
// done in ReceiveMessage()
void CIdarDevice::Update( double sim_time ) 
{
  CPlayerEntity::Update( sim_time ); // inherit some debug output
  
  // UPDATE OUR RENDERING
  double x, y, th;
  GetGlobalPose( x,y,th );
  
  ReMap( x, y, th );
  
  // dump out if noone is subscribed
  if(!Subscribed())
    {
      return; 
    }
  
  m_last_update = sim_time;
}

void CIdarDevice::ClearMessage( void )
{
  // wipe the message (zero the buffer)
  memset( &recv, 0, sizeof(recv) );
#ifdef INCLUDE_RTK2
  // unrender the message 
  rtk_fig_clear(this->data_fig);
#endif
}

void CIdarDevice::TransmitMessage( idartx_t* transmit )
{
  // really should be a valid message here
  assert( transmit && (transmit->len > 0) );
  
  //#ifdef DEBUG
  //printf( "\nTRANSMITTING %d bytes at %p - ",
  //transmit->len, transmit->mesg );
  
  //for( int f=0; f<transmit->len; f++ )
  //printf( "%d ", transmit->mesg[f] );
  //puts ( "" );
  //#endif
  
  // now perform the transmission
  double ox, oy, oth;
  GetGlobalPose( ox, oy, oth ); // start position for the ray trace

#ifdef INCLUDE_RTK2
  rtk_fig_clear(this->rays_fig);
#endif
  
  double scan_start_angle = 
    (oth - m_angle_per_sensor/2.0) + m_angle_per_scanline/2.0;
    
    for( int scanline=0; scanline < m_num_scanlines; scanline++ )
      {
	
	// Compute parameters of scan line
	double scanline_bearing = 
	scan_start_angle + scanline * m_angle_per_scanline;
	
      // normalize to make things easier to read
      scanline_bearing = fmod( scanline_bearing, TWOPI );
	      
      CLineIterator lit( ox, oy, scanline_bearing, m_max_range,
			 m_world->ppm, m_world->matrix,
			 PointToBearingRange );
	      
      //printf( "direction %d scanline %d "
      //      "from %.2f,%.2f %.2fm at %.2f radians\n", 
      //      s, scanline, ox, oy, m_max_range, scanline_bearing );
	      
      CEntity* ent;

      double range = m_max_range;
	      
      // get the next thing in our beam, until the beam is blocked
      while( (ent = lit.GetNextEntity() ) ) 
	{
	  //printf( "HIT %p type %s idar_return %d (i am %p,%d,%d)\n",
	  //ent, ent->token, ent->idar_return,
	  //(CEntity*)this, token, idar_return );
		
	  //printf( "IDARReceive == %d\n", IDARReceive );

	  // if it's not me or my parent or a sibling and it's not
	  // transparent to idar, i'll try sending a message to it
	  if( (ent != (CEntity*)this) && 
	      (ent != this->m_parent_entity) &&
	      (ent->m_parent_entity != this->m_parent_entity) &&
	      ( ent->idar_return != IDARTransparent) )
	    {		    
	      range = lit.GetRange();
	      assert( range >= 0 );
		   
	      //printf( "s:%d.%d range: %.2f\n",
	      //    s, scanline, range );

	      //printf( "hit entity %p (%d:%d:%d)\n",
	      //     ent, ent->m_player.port,  ent->m_player.code,
	      //     ent->m_player.index );

	      uint8_t intensity;
		    
	      switch( ent->idar_return )
		{
		case IDARReceive: // it's a receiver
		  // attempt to poke this message into his receiver
		  //printf( "TRANSMIT to %p type %s idar_return %d\n",
				//ent, ent->token, ent->idar_return );
			
			
		  //PRINT_DEBUG1( "POKING A MESSAGE INTO %p", ent );

		  if( (intensity = LookupIntensity(0,range,false)) > 0 )
		    ((CIdarDevice*)ent)->
		      ReceiveMessage( (CEntity*)this, 
				      transmit->mesg,
				      transmit->len,
				      intensity,
				      false );
		  
		  // continue to next case, since IR receivers are